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ABSTRACT 


Unmanned  Underwater  Vehicles  frequently  rely  on  two-dimensional  sensors  for 
information  about  their  surroundings.  These  sensors  do  not  provide  adequate  information 
for  obstacle  avoidance  in  cluttered  maritime  environments.  To  address  that  issue,  a  three- 
dimensional  reconstruction  of  the  environment  utilizing  occupancy  grids  and  a  prototype 
forward  looking  sonar  will  be  considered.  Providing  the  vehicle  with  three-dimensional 
views  of  the  environment  will  allow  for  optimal  route  planning  and  an  increase  in 
successful  missions  in  complex  environments. 
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EXECUTIVE  SUMMARY 


Unmanned  underwater  vehicles  (UUV)  are  being  tasked  with  a  growing  number 
of  mission  sets.  These  mission  sets  require  the  vehicle  to  perform  tasks  in  a  wide  variety 
of  environments.  The  mission  sets  may  require  the  UUV  to  traverse  up  river  ways, 
perform  searches  in  harbor  environments,  or  navigate  in  littoral  areas.  In  each  of  these 
areas,  the  complexity  of  the  environment  poses  challenges  for  the  safe  navigation  of  the 
vehicle. 

The  current  build  of  UUVs  are  utilizing  sensors  that  provide  two-dimensional 
representations  of  their  operating  environment.  In  compressing  the  infonnation  from  a 
three-dimensional  world  into  a  two-dimensional  representations  a  significant  amount  of 
information  is  lost.  The  loss  of  this  information  creates  challenges  in  the  proper 
navigation  of  the  vehicles.  The  main  goal  of  this  work  is  to  provide  the  vehicle  with  a 
three-dimensional  representation  of  the  environment  that  can  be  utilized  for  obstacle 
avoidance  and  route  planning. 

With  this  goal  in  mind,  forward  looking  sonar  (FLS)  was  chosen  as  the  sensor  of 
interest.  This  research  mounted  two  prototype  FLS  onto  the  Remote  Environmental 
Measuring  Units  (REMUS)  used  by  the  Center  for  Autonomous  Vehicle  Research 
(CAVR)  at  the  Naval  Postgraduate  School.  One  of  the  sensors  provided  a  horizontal 
field  of  view  that  covered  ninety  degrees  and  ninety  meters  in  front  of  the  vehicle.  The 
horizontal  FLS  also  has  fifteen  degrees  of  ambiguity  in  the  vertical  direction.  The  other 
FLS  was  mounted  to  provide  a  vertical  field  of  view  that  covers  forty-five  degrees  and 
ninety  meters,  with  fifteen  degrees  of  ambiguity  in  the  horizontal  direction. 

An  occupancy  grid  was  used  as  the  framework  of  the  reconstruction.  Occupancy 
grids  divide  the  area  into  sub  regions.  Each  region  or  cell  is  assigned  a  probability  that 
indicates  the  likelihood  of  that  cell  being  occupied.  The  probabilities  of  occupancy  are 
determined  from  physical  and  probabilistic  models  of  the  sensors.  Each  measurement 
received  by  the  sensors  is  converted  into  a  probability  of  occupancy  and  provides  an 


update  to  the  overall  probability  stored  in  the  grid.  With  enough  measurements,  a  clear 
three-dimensional  representation  can  be  reconstructed. 

To  prove  the  applicability  of  this  method,  REMUS  was  deployed  in  the  Charles 
River  in  Boston,  Massachusetts  and  directed  underneath  the  Massachusetts  Avenue 
Bridge.  The  bridge  provided  obstacles  that  have  distinct  three-dimensional  shapes. 
Using  this  dataset  and  the  models  and  algorithms  developed  the  main  features  of  the 
bridge,  the  benn  and  pylons  were  reconstructed.  The  resulting  three-dimensional  grid 
clearly  depicted  the  height,  width,  and  depth  of  the  pylons.  The  grid  was  also  able  to 
locate  the  rapid  shoaling  of  the  benn. 

The  two-dimensional  representations  of  the  bridge  depict  the  bridge  as  an 
unnavigable  area.  By  successfully  locating  these  obstacles  and  determining  the  open 
space  surrounding  them,  the  vehicle  can  safely  navigate  underneath  the  bridge.  Three- 
dimensional  reconstruction  of  the  environment  extends  the  areas  in  which  the  vehicle  can 
safely  be  deployed. 
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I.  INTRODUCTION 


A.  MOTIVATION 

During  the  last  several  years  there  has  been  an  explosion  of  interest  in  unmanned 
underwater  vehicles  (UUV).  UUVs  are  being  utilized  to  explore  the  ocean  depths,  survey 
and  aid  in  salvage  operations,  and  are  even  used  in  keeping  our  soldiers  and  sailors  safe. 
The  current  crop  of  UUVs  is  utilized  in  areas  where  the  main  navigational  hazard  is  the 
ocean  bed.  The  next  generation  of  UUVs  will  need  to  be  able  to  navigate  in  water  with  a 
variety  of  obstruction:  pier  pylons,  kelp  beds,  oil  platfonns,  etc. 

B.  STATEMENT  OF  PROBLEM 

UUVs  are  currently  limited  by  the  navigational  sensors  they  employ.  The  sensors 
primarily  map  the  environment  into  a  two-dimensional  representation.  While  this  works 
well  for  unmanned  ground  vehicles  mapping  a  hallway,  it  leads  to  many  complications 
when  traversing  in  the  undersea  environment.  In  the  past  UUVs  have  been  employed  in 
locations  with  a  large  open  area.  They  have  successfully  mapped  the  bottom  contours 
and  other  bathymetric  features.  However,  the  next  generation  of  UUVs  will  need  the 
ability  to  navigate  among  a  wider  variety  of  obstacles.  For  example,  a  UUV  may  be 
required  to  conduct  searches  in  a  congested  harbor  area  and  do  so  several  times  a  year  to 
measure  and  detect  changes  in  the  environment.  This  adds  a  new  level  of  navigational 
complexity. 

In  order  to  safely  navigate  in  an  unknown  environment  the  vehicle  must  be  able  to 
image  the  space  directly  in  front  of  it.  Forward  Looking  Sonar  (FLS)  can  provide 
submerged  vehicles  with  this  capability.  Current  FLS  map  the  area  into  two-dimensional 
images.  During  the  process  of  compressing  a  three-dimensional  world  into  a  two- 
dimensional  image  infonnation  is  lost.  In  some  circumstances,  that  information  can  be 
critical  for  proper  navigation. 

Figure  1  is  a  picture  of  the  Massachusetts  Avenue  Bridge  in  Boston  MA.  Figure  2 
is  a  rough  approximation  of  the  undersea  support  for  the  bridge.  There  are  several  distinct 
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features.  Each  pylon  has  an  associated  width,  height  and  depth.  The  river  bottom  quickly 
shoals  on  each  side  of  the  bridge.  The  shoaling  on  either  side  is  to  support  the  pylons. 


Figure  1 .  Massachusetts  Avenue  Bridge  in  Boston  MA, 

from  [18] 


Figure  2.  Submerged  Bridge  Pylons 
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Currently,  UUVs  are  using  FLS  for  obstacle  avoidance  and  path  planning  ([1] 
[2]).  The  NPS  REMUS  vehicle  is  currently  equipped  with  both  a  vertical  and  horizontal 
FLS.  Figures  3  and  4  show  both  horizontal  and  vertical  FLS  sonar  images  from  a  UUV 
passing  underneath  the  bridge. 


Figure  3.  Horizontal  FLS  Image  from  a  REMUS  vehicle  navigating  underneath  the 

Massachusetts  Avenue  Bridge 


Figure  4.  Vertical  FLS  Image  from  a  REMUS  vehicle  navigating  underneath  the 

Massachusetts  Avenue  Bridge 
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The  first  feature  of  note  is  the  quickly  shoaling  river  bottom.  According  to  the 
horizontal  FLS,  the  change  in  depth  of  the  river  bottom  appears  as  a  solid  wall. 
However,  data  provided  by  the  vertical  FLS  shows  that  the  vehicle  can  simply  change  its 
depth  and  safely  navigate  above  the  river  bottom.  The  infonnation  from  the  horizontal 
image  would  prevent  the  vehicle  from  approaching  the  bridge.  The  pylons  of  the  bridge 
present  a  similar  problem.  They  are  clearly  visible  in  the  horizontal  image,  but  again 
show  as  a  solid  object  in  the  vertical  image.  This  registers  with  the  obstacle  avoidance 
algorithm  as  a  rigid  boundary. 

The  main  point  is  that  when  relying  either  on  the  individual  vertical  and 
horizontal  images  the  vehicle  would  not  be  able  to  find  a  safe  path  for  navigation  under 
the  bridge.  The  bridge  effectively  becomes  a  blockade  that  prevents  further  navigation. 
This  is  a  severe  limitation  for  UUVs.  In  order  to  overcome  this,  a  method  must  be 
developed  to  take  the  two  separate  sources  of  information  and  merge  them  into  a  more 
coherent  single  three-dimensional  environmental  model.  A  model  that  allows  the  vehicle 
to  locate  the  opening  between  the  pylons. 

UUVs  are  being  called  upon  to  accomplish  increasingly  complex  tasks,  such  as 
navigating  up  rivers.  This  requires  detailed  knowledge  of  the  three-dimensional  world  in 
which  the  UUV  is  located.  In  order  to  accomplish  tasks  such  as  these,  the  vehicle  will 
need  the  benefits  provided  by  a  three-dimensional  model  of  the  area. 

C.  SCOPE  AND  STRUCTURE  OF  THESIS 

This  thesis  will  look  into  the  feasibility  of  reconstructing  a  three-dimensional 
environment  through  the  use  of  two-dimensional  horizontal  and  vertical  forward  looking 
sonar  images.  The  creation  of  the  three-dimensional  environment  is  considered  critical 
for  the  UUV  to  navigate  in  cluttered  and  restricted  waterways.  In  order  to  create  a  three- 
dimensional  model,  a  probabilistic  model  of  the  forward-looking  sonar  will  be  developed. 
There  are  two  aspects  to  the  model  -  the  vehicle  motion  model  and  the  sensor  model.  The 
sensor  information  is  only  as  good  as  the  estimate  of  the  vehicle’s  position.  The  thesis 
will  start  with  a  description  of  the  vehicle  and  a  probabilistic  model  for  estimating 
position  and  errors. 
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Next,  the  sensor  model  will  be  defined.  The  model  will  define  the  spatial 
relationship  described  by  the  sonar  images.  This  relationship  will  be  used  to  detennine 
an  object’s  location  in  global  enviromnent.  This  FLS  model  will  also  need  to  define  both 
the  probability  of  detection  and  the  probability  of  noise  attributed  to  each  sensor. 

Following  the  FLS  model  description,  an  occupancy  grid  will  be  developed.  This 
three-dimensional  grid  will  be  the  representation  of  the  environment.  The  probabilistic 
model  of  the  vehicle’s  position  will  be  combined  with  the  probabilistic  and  spatial 
models  of  the  FLS  to  populate  the  occupancy  grid.  The  following  chapters  will  discuss 
the  generation  of  the  FLS  model,  the  basis  for  occupancy  grids,  and  will  culminate  with 
the  algorithm  used  to  create  the  three-dimensional  model. 

D.  PRIOR  WORK 

Three-dimensional  mapping  has  been  considered  for  many  applications.  Robotic 
vehicles  have  employed  various  techniques  to  detennine  their  environment,  whether  they 
are  entering  damaged  buildings  for  disaster  relief  [3]  or  mapping  the  ocean  floor  [4]. 
Unmanned  vehicles  have  also  been  employed  with  a  wide  range  of  sensors.  The  sensors 
utilized  in  three-dimensional  mapping  include  cameras,  laser  range  finders,  radar,  and 
multiple  fonns  of  sonar. 

The  current  methods  used  in  mapping  the  undersea  environment  are  focused  on 
producing  maps  after  the  vehicle  has  transited  through  the  area.  These  methods  typically 
involve  a  side  scan  sonar.  Side  scan  sonar  uses  a  specific  array  of  active  sonar  heads  to 
provide  imagery  of  the  ocean  floor  to  the  side  of  an  UUV.  Side  Scan  sonar  can  produce 
very  accurate  three-dimensional  measurements,  but  will  not  provide  images  of  the 
volume  directly  in  front  of  the  vehicle.  With  the  goal  of  obstacle  avoidance,  the  use  of 
side  scan  sonar  provides  little  benefits.  Forward-Looking  Sonars  have  previously  been 
used  for  reactive  obstacle  avoidance  with  success  ([1]&  [2]).  Nevertheless,  in  this  past 
research  the  vehicle  was  limited  to  a  singular  plane  of  view,  either  vertical  or  horizontal. 
Little  research  has  been  conducted  on  combining  multiple  FLS  into  a  single 
representation.  The  thesis  will  look  into  the  feasibility  of  utilizing  FLS  to  develop  a 
three-dimensional  map. 
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II.  VEHICLE  DESCRIPTION 


A.  VEHICLE  DESCRIPTION 

The  Remote  Environmental  Measuring  Units  (REMUS)  is  the  vehicle  used  in  this 
thesis.  It  is  a  multipurpose  unmanned  underwater  vehicle  (UUV)  currently  in  use  by  the 
Center  for  Autonomous  Vehicle  Research  (CAVR)  at  the  Naval  Postgraduate  School 
(NPS).  REMUS  is  a  small  man-portable  UUV  ([5]).  REMUS  specifications  are  listed  in 
Table  1.  This  small  package  provides  for  easy  deployment  and  recovery.  Coupling  its 
ease  of  deployment  with  a  reconfigurable  payload  allows  REMUS  to  be  capable  of 
executing  a  wide  assortment  of  missions. 


Maximum  Diameter 

19  cm 

Maximum  Length 

160  cm 

Weight  in  Air 

37  kg 

Trim  Weight 

1kg 

Maximum  Operating 

Depth 

100m 

Energy 

lkw-hr  Lithium  Ion  Battery 

Endurance 

22  hours  at  3  kts 
>8  hours  at  5  kts 

Propulsion 

DC  brushless  motor,  3  bladed 
propeller 

Velocity 

Up  to  5  kts 

Table  1.  REMUS  Specifications,  from  [5] 
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Figure  5.  NPS  REMUS  onboard  The  Cypress  Sea 


REMUS  can  be  equipped  with  various  environmental  sensors  including 
temperature  and  conductivity  sensors,  pressure  sensors,  fluorometer,  and  turbidity 
sensors.  In  addition  to  the  various  environmental  sensors,  it  is  also  capable  of  carrying  a 
wide  range  of  imaging  sensors,  to  include  side  scan  sonar,  forward  looking  sonar  (FLS), 
and  video  cameras.  Navigationally,  the  vehicle  may  choose  from  many  options.  The 
navigational  sensors  include  GPS,  inertial  navigational  system  (INU),  long  or  ultra  short 
baseline,  and  Acoustic  Doppler  Current  Profiler  (ADCP).  This  wide  range  of  payload 
options  allows  REMUS  to  be  used  in  operations  that  range  from  scientific  surveys  to 
harbor  security. 

The  REMUS  vehicle  employed  by  the  CAVR  is  a  modified  version.  The  CAVR 
REMUS  is  equipped  with  a  prototype  FLS  provided  by  BlueView  Technologies.  The 
prototype  FLS  will  be  the  main  sensor  utilized  in  this  thesis.  This  FLS  will  be  discussed 
at  length  in  the  following  chapters. 
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B.  EQUATIONS  OF  MOTION 


To  take  full  advantage  of  the  sensors  employed,  the  position  of  the  vehicle  is 
required.  Like  all  vehicles,  REMUS  position  at  any  given  time  is  a  result  of  a  complex 
set  of  forces.  A  single  paper  could  be  dedicated  to  the  description  of  the  forces  affecting 
underwater  vehicles.  The  purpose  of  this  section  is  to  provide  the  reader  with  a 
simplified  overview  of  these  forces  and  how  they  are  modeled.  A  more  detailed 
explanation  can  be  found  in  [7]. 

To  discuss  the  forces  acting  upon  a  body,  a  common  reference  frame  must  be 
defined.  The  standard  approach  to  relate  the  position  and  motion  in  two  separate  frames 
of  reference  is  the  Newton  Euler  approach.  The  coordinate  systems  used  in  this  approach 
can  be  seen  in  Figure  6  . 


Global  Frame 


Figure  6.  Local  and  Global  Coordinate  System,  from  [7] 

The  following  assumptions  are  made  for  this  model: 

■  The  rotation  of  the  earth  does  not  appreciably  contribute  to  the 
acceleration  of  the  vehicles  center  of  mass.  Therefore,  the  rotation  of  the 
earth  is  negligible. 

■  The  vehicle  is  modeled  as  a  rigid  body 
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The  sources  of  significant  forces  acting  upon  the  vehicle  are  propulsion, 
hydrostatic,  and  hydrodynamic  lift  and  drag.  The  forces  can  all  be 
classified  as  either  inertial  or  gravitational. 


The  local  velocity  is  a  vector  composed  of  surge  (u),  sway  (v),  and  heave  (w). 

r x  y  z~\ 

The  conversion  from  the  global  velocity  L  -Uo  the  local  velocity  requires  a 

transfonnation  matrix. 


X 

~x~ 

y 

_  Y  * 

Y 

z 

Z 

2-1 


T,  the  transformation  matrix,  is  defined  in  terms  of  ‘Euler’  angles  ((p,0,v|/)  and  is 
listed  in  Equation  2-2. 


T(<p,d,y. '/) 


cos  y/  cos  6 

cos  y/  sin  dO  sin  (j)  -  sin  y/  cos  (j) 
cos  y/  sin  G  cos  (j)  +  sin  y/  sin  (j) 


sin  y/  cos  6  -sin  0 

sin  y/  sin  0  sin  (/>  +  cos  y/  cos  (j)  cos  G  sin  (j) 
sin  y/  sin  G  cos  (j)-  cos  y/  sin  (j)  cos  G  cos  (j) 


2-2 


Likewise  the  rate  of  change  of  the  ‘Euler’  angles  can  be  defined  in  terms  of  the 
global  angular  velocity  vector  [p,q,r]. 


1 

sin  (j)  tan  0 

cos  (j)  tan  0 

P 

0 

= 

0 

COS  (j) 

-sin  (j) 

q 

2-3 

¥_ 

0 

sin  (/>/ 

/ cos  6 

COS(/>  / 

/ COS0_ 

r 

From  these  equations,  Healey  derived  six  equations  of  motion  for  a  rigid  body. 
The  equations  of  motion  defined  as  Surge  (Equation  2-4),  Sway  (Equation  2-5),  Heave 
(Equation  2-6),  Roll  (Equation  2-7),  Pitch  (Equation  2-8),  and  Yaw  (Equation  2-9). 
Table  2  lists  the  variables  and  definitions  used  in  the  equations  of  motions. 
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Xf=m  ur  -vrr  +  wrq  -  xG(q2  +  r2)  +  yG{pq  -r)  +  zG{pr  +  q)  +  (W-B)sm6  2-4 

Yf=m  vr+urr-wrq  +  xG(pq  +  r)-yG(p2 +  r2)  +  zc(pr-q)  -  (W  -  B)  cos  6  sin  (j)  2-5 

Zf=m  wr  -urq  +  vrp  +  xG(pr  +  q)  +  yG(qr  +  q)- zG(p2  +  q2)  +  {W  -  B)cos0cos9  2-6 

Kf  =  PP  +  (P  ~  P ) vr  +  4  (pr  -q)~  4  W  ~r2)~  4  {pv  + r) 

+m[yG(w-urq  +  vrp)-zG(vr  +urr-wrp)~\  2-7 

-(yGW  -  ybB^  cos  Geos  (j)  +  (zGW  -  zbB)  cos  0 sin  (/> 

Mf=  I  A  +  (P  -  P )  pr  -  Py  (qr  -p)  +  Iyz  ( pq  -  r)  +  Ixz  (p1  +  r2 ) 

-m [xG(w- urq  +  vrp )  -  zG  (ur  - vr  +  wrQ)]  2-8 

+  (xGW  +  xbB)  cos  6  cos  (j)  +  (zGW  -  zbB)  sin  (f) 

Nf  =  I.r  +  (/,,  -  Ix  )  pq  -  4,  (p2  -q2)-  Iyz  ( pr  +  q)  +  Ixz  (qr  -  p) 

+m[xG  (vr  +  urr  -  wrp )  -  yG  ( ur  -  vrr  +  wrqj\  2-9 

-^XqW  -  xhB) cos 9 sm(j)  -  -  yBBpinO 


Ix?  ly?  Iz 

Mass  moment  of  inertia  terms 

Ur,  Vr,  Wr 

Component  velocities  for  a  rigid  body  fixed 
system  with  respect  to  the  water 

P,  q,r 

Component  angular  velocities  for  a  rigid  body 
fixed  system 

xb,  Yb,  Zb 

Positional  difference  between  center  of 
buoyancy  and  the  geometric  center 

xg,  Yg,zg 

Positional  difference  between  the  center  of 
gravity  and  the  geometric  center 

B 

Buoyancy 

W 

Weight 

5r(t) 

Delta  Rudder  function 

Table  2 .  Equation  of  Motion  V ariables 


Dolbec  [8]  coupled  the  equations  of  motion  with  the  hydrodynamic  coefficients 
associated  with  REMUS  [9]  to  create  the  final  kinematics  equation  used  to  model  the 
motion  of  REMUS  (Equation  2-10). 
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8r{t)  2-10 


1 

1 

0 

0" 

u 

'4 

Yr  -  mU 

0" 

u 

i 

1 _ 

0 

4  -4 

0 

r 

= 

4, 

4 

0 

r 

+ 

4 

0 

0 

1 

_v\ 

0 

0 

1 

y  _ 

0 

C.  POSITION  ESTIMATION 

The  equations  of  motion  provide  the  theoretical  framework  for  determining  a 
vehicle’s  position  based  upon  the  forces  acting  upon  it.  For  the  equations  of  motion  to 
provide  any  practical  benefit  they  must  be  coupled  with  a  sensor  capable  of  measuring 
the  acceleration  and  angular  rates  in  each  of  the  component  directions.  For  a  UUV  the 
sensors  include  the  following: 

1.  Acoustic  Doppler  Current  Profiler  (ADCP)/Doppler  Velocity  Log  (DVL) 

-  It  provides: 

a.  Water  velocities  -  surge,  sway  and  heave 

b.  Vehicle  velocities  over  the  ground  -  forward,  side  slip  and  heave 

c.  Vehicle  Altitude 

2.  Depth  sensor 

3.  Compass  -  Heading  and  heading  rate 

4.  Global  Positioning  System  -  Latitude  and  Longitude  position  on  the 

surface 

5.  Inertial  Navigation  Unit  (INU)  -  Accurate  angular  and  motion  velocities 


These  sensors  are  combined  together  using  an  Extended  Kalman  Filter  (EKF)  to 
provide  an  optimal  position  estimation  of  the  vehicle.  The  theory  of  the  EKF,  a 
derivative  of  the  Kalman  filter,  has  been  discussed  in  depth  in  many  publications;  this 
chapter  will  serve  simply  as  an  overview  of  the  EKF  and  its  application  to  REMUS. 

In  a  Kalman  filter,  the  state  and  variance  of  a  dynamic  linear  system  is  determined 
recursively.  The  Kalman  filter  is  a  two-step  process.  The  first  step  is  to  predict  the  state 
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and  variance  of  the  system  using  the  model  of  the  system  (movement  update).  For 
REMUS  the  equations  of  motion  serve  as  this  model.  Upon  receiving  a  new 
measurement,  the  difference  between  the  measurement  and  the  predicted  state  is 
calculated.  This  difference  is  a  correction  factor  used  to  update  the  current  state  of  the 
system  (measurement  update).  The  process  is  depicted  in  Table  3. 


Movement 

Update 

Mi+ 1  =  M+UW 

2/+i 

Measurement 

Update 

'+l  ITLmH'+R 

Mm\z  =Mm  +K{Z-Mv~H/j) 

Variable 

Definition 

mean  and  covariance  of  the  system  state 
(  liv  ,R  )  \  mean  and  covariance  of  the  measurement  noise 
( jnw,Q) :  mean  and  covariance  of  the  movement  noise 

H:  measurement  matrix 

K:  Kalman  Gain 

Z:  measurement 

(j) :  movement  matrix 

Table  3.  Kalman  Filter  Equations,  after  [10] 


The  method  listed  in  Table  3  will  only  work  for  systems  with  a  linear  model. 
Flowever,  modeling  the  position  of  REMUS  is  a  non-linear  process.  Since  REMUS  does 
not  measure  position  directly,  but  deals  with  the  estimation  of  position  from  both  angular 
rates  and  accelerations  the  equations  of  motion  are  non-linear  in  nature.  Because  of  the 
non-linearity  an  EKF  is  required.  In  an  EKF,  the  nonlinear  components  are  linearized  by 
an  approximation  function  (normally  a  Taylor  series  approximation).  In  an  EKF  the 
following  substitutions  are  made: 
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■  ([X,  Z)  is  replaced  with  f(p,  Z) 

■  Z  is  replaced  with  h(Z) 

The  most  accurate  sensor  for  measuring  the  global  position  of  the  REMUS  was  GPS. 
This  is  only  available  when  the  vehicle  is  on  the  surface.  Once  underwater,  position 
estimation  errors  grow  more  rapidly  over  time.  For  the  UUV  to  navigate  accurately  for 
extended  periods  of  time  underwater,  it  requires  a  high-grade  inertial  navigation  system. 
The  REMUS  UUV  had  an  integrated  navigation  suite  that  combines  together  the  IMU, 
GPS  and  ADCP/DVL.  It  is  the  SEA  DeViL  T24,  a  navigation  suite  from  Kearfott  that 
advertises  a  position  accuracy  of  0.5%  total  distance  traveled.  The  stated  manufacture 
specifications  for  REMUS’  IMU  are  listed  in  Table  4.  Alameda  provides  a  detailed 
explanation  of  the  Sea  DeViL  architecture  [11]. 


Surface  Ship 

SEA  DeViL  PERFORMANCE* 

KN-6051  1  KN-6052 

KN-6053 

Position  Accuracy 

•  GPS/DVL" 

•  DVL  (Water-Track  Mode)"” 

10  m,  CEP 

10  nm/8hrs,  TRMS 

10  m,  CEP 

2  nm/8hrs,  TRMS 

10  m,  CEP 

1  nm/8hrs,  TRMS 

Heading  Accuracy 

•  GPS/DVL" 

•  DVL 

5.0  mils,  rms 

5  mils'  secant  X,  rms 

<1.5  mils,  mis 

1 .0  mils'  secant  k,  rms 

<1.0  mils,  rms 

0  5  mils  ’  secant  X,  rms 

Velocity  Accuracy 

•  GPS/DVL" 

•  DVL  (Water-Track  Mode) 

Roll/Pitch  Accuracy 

0.05  m/sec 

0.5  m/sec,  rms 

0.5  mils,  rms 

0.05  m/sec 

0.35  m/sec,  rms 

0.5  mils,  rms 

0.05  m/sec 

0.3  m/sec,  rms 

0.5  mils,  rms 

Underwater  Vehicle*** 

KN-6051 

KN-6052 

KN-6053 

Position  Accuracy  ”" 

0.5%  DT,  CEPR 

0  2%  DT,  CEPR 

0.05%  DT,  CEPR 

Heading  Accuracy 

5  mils',  rms 

1 .5  mils',  rms 

1.0  mils ',  rms 

Roll/Pitch  Accuracy 

0  5  mils,  rms 

0.5  mils,  rms 

0.5  mils,  rms 

Table  4.  Sea  DeViL  Performance  From  Kearfott  Systems,  from  [12] 


During  this  experiment,  the  vehicle  did  not  receive  any  position  measurements 
after  the  initial  dive.  The  position  and  the  errors  associated  with  it  will  be  entirely 
attributed  to  the  accuracy  of  the  Sea  DeViL.  Due  to  the  short  relative  distance  traveled 
during  experiment,  the  errors  in  position  are  minimal.  The  vehicle  traveled 
approximately  450  meters  to  reach  the  bridge.  This  equates  to  a  possible  positional  error 
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of  2.3  meters.  The  total  distance  traveled  at  the  beginning  of  the  last  pass  underneath  the 
bridge  was  1350  meters,  yielding  a  maximum  expected  error  of  6.5  meters.  For  this 
reason,  the  position  and  orientation  will  be  assumed  to  be  relatively  accurate. 

The  position  of  the  vehicle  (or  mobile  sensor)  and  its  orientation  play  a  critical 
role  in  mapping  the  output  of  the  sensor  to  the  global  space.  All  of  the  information 
provided  by  the  sensor  is  referenced  to  the  vehicles  location  and  attitude.  Any  error  in 
the  vehicles  location  is  propagated  through  each  of  the  models.  In  other  words,  errors  in 
the  vehicle’s  position  or  orientation  will  cause  objects  to  be  plotted  in  the  wrong  location, 
invalidating  the  entire  method. 

The  next  chapter  will  discuss  the  model  of  the  FLS.  The  FLS  model  will  then  be 
combined  together  with  the  vehicle  model  in  order  to  take  the  sonar  images  and 
reconstruct  a  three-dimensional  occupancy  grid  that  represents  underwater  objects. 
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III.  FORWARD  LOOKING  SONAR 


A.  INTRODUCTION 

A  sonar  uses  the  distribution  and  reflection  of  a  sound  wave  to  detennine  the 
range  and  bearing  to  submerged  objects.  Active  sonar  systems  use  a  transducer  to  create 
their  own  source  of  sound  in  the  water  (called  a  ping)  and  then  listen  for  its  return.  In  its 
most  basic  form,  active  sonar  determines  the  distance  to  a  given  target  by  measuring  the 
time  delay  from  the  production  of  a  sound  until  the  return  reaches  the  transducer.  In 
order  to  determine  the  distance  to  a  target,  all  that  is  required  is  the  total  travel  time  and 
the  speed  of  sound  through  the  water.  This  yields  Equation  3-1. 

„  time  delay  *  Speed  of  sound  „  , 

Range  = - - — ^ -  3  - 1 

The  accuracy  of  the  range  is  then  dependent  upon  the  ability  of  the  system  to 
accurately  measure  the  time  delay  and  the  accuracy  of  the  speed  of  sound.  Ideally,  the 
speed  of  sound  would  be  constant  throughout  the  water,  however,  in  practice  this  is  not 
the  case.  The  approximate  speed  of  sound  is  calculated  onboard  the  REMUS  vehicle 
using  water  temperature  and  salinity.  Normally,  this  is  about  1500  meters  per  second. 

Ranging  information  alone  is  not  sufficient  to  geo-locate  an  object.  To  accurately 
determine  the  location  of  the  object  it’s  bearing  must  also  be  detennined.  To  measure 
the  bearing  to  an  object,  a  transducer  will  only  project  or  listen  to  sounds  along  a 
particular  bearing  (also  called  a  beam).  Thus  by  only  listening  to  sounds  along  a 
particular  bearing  and  measuring  the  time  delay  of  the  sounds  return,  both  the  bearing 
and  the  range  to  an  object  can  be  detennined.  The  ability  of  the  transducer  to  listen  along 
a  particular  bearing  or  range  of  bearings  is  called  its  directivity.  A  transducer  with  a  high 
directivity  will  be  able  to  isolate  sounds  to  a  small  range  of  bearings.  Transducers  with 
low  directivity  have  larger  bearing  resolutions. 

For  the  sonar  to  determine  the  range  and  the  bearing  to  an  object  it  must  be  able  to 
distinguish  the  return  from  the  background  noise.  There  are  numerous  sources  of  noise 
for  any  sonar  system.  In  the  underwater  environment,  multiple  sources  of  sound  exist. 
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Everything  from  biologies  to  merchant  traffic  emits  sounds  across  all  frequencies.  Wave 
action  and  rain  also  contribute  significantly  to  the  amount  of  background  noise  a  sonar 
system  must  overcome.  The  overall  ability  of  the  sonar  system  to  determine  detection  is 
based  on  the  ratio  of  return  signal  received  at  the  transducer  to  the  amount  of  general 
noise  received  at  the  transducer.  This  is  the  sonar’s  signal  to  noise  ratio  (SNR). 

Due  to  the  larger  range  of  values  of  sound  energy  in  the  water,  SNR  is  generally 
defined  in  tenns  of  decibels  (dB).  The  general  equation  for  SNR  in  an  active  sonar 
system  is  shown  in  Equation  3-2. 

SNR  =  SL-2TL  +  TS  -  NL  3.2 

The  amount  of  signal  returned  to  the  transducer  is  dependent  upon  the  amount  of 
sound  initially  projected  into  the  water,  the  source  level  (SL).  After  leaving  the 
transducer,  the  sound  expands  and  travels  through  the  medium  towards  the  object.  While 
traveling,  the  intensity  of  the  sound  is  diminished  via  attenuation  and  expansion.  This 
loss  of  intensity  is  called  the  transmission  loss  (TL).  The  total  amount  of  sound  received 
at  the  object  is  the  source  level  minus  the  transmission  loss. 

Once  the  sound  reaches  an  object  in  the  water,  it  is  then  reflected  off  the  object. 
The  amount  of  sound  reflected  and  directed  back  towards  the  original  source  of  the  sound 
is  called  the  target  strength  (TS).  Many  factors  affect  the  amount  of  sound  reflected  off 
the  object.  TS  is  dependent  upon  the  angle  of  incidence,  the  absorption  properties  of  the 
object,  the  size  and  shape  of  the  object,  etc.  Once  the  sound  is  reflected,  it  travels  back 
towards  the  source.  As  it  travels,  it  undergoes  the  same  amount  of  transmission  loss.  So 
the  total  pressure  received  at  the  transducer  from  original  signal  is 

Received  Signal  =  SL  -  2  TL  +  TS  3-3 

However,  this  does  not  take  into  account  the  amount  of  noise  the  transducer 
receives.  Letting  the  noise  level  (NL)  be  defined  as  the  amount  of  noise  received  by  the 
transducer,  the  SNR  equation  becomes 

SNR  =  SL  -2TL  +  TS  -  NL  3-4 
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B. 


BLAZED  SONAR  ARRAYS 


Blazed  array  sonar  is  a  method  to  create  a  highly  directive  signal  in  a  small 
package.  Blazed  array  sonar  systems  utilize  a  principle  similar  to  echelette  diffraction 
gratings  in  optics  to  create  a  multitude  of  beams  from  a  single  sound  source  [13].  The 
blazed  sonar  array  maps  the  frequency  of  a  given  broadband  pulse  to  the  angular  spatial 
domain.  The  principle  is  similar  to  that  of  a  prism.  A  broad  spectrum  signal  (white  light) 
is  processed  through  the  stave  (the  prism)  and  individual  frequencies  are  projected  to 
independent  spatial  locations  (the  rainbow).  Each  frequency  band  creates  a  unique  beam. 
The  shape  and  size  of  the  beam  is  dependent  on  the  frequency  band  and  the  shape  of  the 
stave  projecting  the  sound.  A  plot  of  the  magnitude  of  the  projected  sound  versus  bearing 
for  a  typical  blazed  array  sonar  is  provided  in  Figure  7.  The  lower  larger  beams  were 
created  by  the  lower  frequencies. 


300  kHz  600  kHz 


Figure  7.  Composite  Blazed  array  beam  patterns  for  frequencies  between  300  kHz 

and  600  kHz,  from  ([15]) 
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Once  the  sound  has  been  transmitted  into  the  water  from  the  blazed  array,  it 
interacts  with  its  enviromnent  just  like  any  other  active  sonar.  The  main  difference  is  that 
the  frequencies  are  separated  into  individual  beams.  As  all  the  frequencies  return  to  the 
transducer,  they  are  recombined  into  a  single  signal.  The  recombination  of  the  signals 
occurs  in  reverse  order  of  the  creation.  Each  beam  will  only  receive  a  particular  band  of 
frequencies.  This  maintains  the  original  frequency  to  angular  spatial  mapping. 

Since  each  frequency  band  correlates  to  a  specific  angle  in  the  spatial  domain,  the 
individual  frequencies  can  be  modeled  as  a  single  hydrophone  with  a  high  directivity. 
Maintaining  the  frequency  to  angular  spatial  relationship  creates  a  simulated  array  of 
hydrophones.  This  simulated  array  is  what  creates  the  detailed  representation  of  the 
volume  ensonified. 

C.  BLUE  VIEW  P450-15E 

The  BlueView  P450-15E  is  a  blazed  array  system  with  a  center  frequency  of  450 
kHz.  The  P450-15E  specifications  are  listed  in  Table  5. 

P450E-15  Sonar 


Max  Range: 

450ft 

Update  Rate: 

Up  to  10Hz 

Swath  Width 

45° 

Beam  Width 

1°  x  15° 

Electrical 

Power 

12-48  volts  @  10  watts  (with  50ft  cable) 

Communications 

Ethernet,  lObase-T  or  lOObase-T 

Mechanical 

Depth  Rating 

1000ft 

Weight  in  air 

5.75  lbs 

Weight  in  fresh 
water 

1.4  lbs 

Dimensions  (Max) 

9.5”  x  7"  x  4" 

Acoustic 

Operating  Frequency 

450kHz 

Table  5.  BlueView  P450-15E  Specifications  (from  BlueView  Inc.,  2008) 
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The  P450-15E  produces  image  files  in  both  Cartesian  and  Polar  coordinates  (see 
Figure  8).  The  resolutions  available  according  to  image  type  are  listed  in  Table  6. 
While  the  P450-15E  is  capable  of  higher  range  resolutions,  it  is  constrained  by  the  size  of 
the  image  available.  Due  to  the  nature  of  the  blazed  array  sonar,  the  polar  coordinate 
system  is  the  natural  coordinate  system.  In  order  to  transfer  the  information  from  the 
polar  coordinate  system  to  the  Cartesian  image,  pixels  with  changing  resolutions  were 
required.  In  the  polar  image  the  resolution  per  pixel  is  constant  throughout  the  entire 
image.  For  these  reasons,  polar  images  where  utilized  in  this  thesis. 


Cartesian  Image 


Polar  Image 


Figure  8.  Cartesian  and  Polar  images  from  P450-15E  sonar 


BlueView 

Specification 

Cartesian 

Polar 

Image  Size  (in 
pixels) 

387x571 

461 x 2048 

Range 

Resolution 

2  inches 

Appoximately 

12”  x  6” 

90  m 

.  r  i  ,  /  .O  /  /pixel 

461  pixels 

Bearing 

Resolution 

1  deg 

varies  based  on 
the  location  of 
the  pixel 

90° 

yv  _  r\AA  deg/ 

2048  pixels  '  /pad 

Table  6.  Comparison  of  Horizontal  FES  Image  Resolutions 
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Both  image  files  provide  an  angular  resolution  greater  than  the  manufacture 
specifications.  This  anomaly  is  attributed  to  the  varying  beamwidths  of  the  blazed  array 
sonar.  The  larger  bearing  resolution  listed  by  the  manufacture  corresponds  to  the  larger 
beamwidth  of  the  lower  frequency  beam.  However,  the  resolution  of  the  polar  image 
appears  to  be  based  upon  the  beamwidth  of  the  highest  frequency  beam. 

The  image  created  by  a  P450-15E  is  a  two-dimensional  representation  of  a  three- 
dimensional  volume.  A  depiction  of  the  actual  volume  ensonified  is  depicted  in  Figure  9. 
A  single  stave  of  the  P450-15E  has  an  approximate  23-degree  field  of  view  in  the  image 
plane.  The  NPS  REMUS  AUV  has  four  staves  in  the  horizontal  plane  for  an  approximate 
90-degree  total  field  of  view. 


Figure  9.  Volume  ensonified  by  the  horizontal  P450-15E 


The  ensonified  volume  covers  a  fifteen  degree  spread  in  the  vertical  plane.  The 
sonar  is  unable  to  distinguish  the  difference  between  an  object  at  different  elevations. 
For  example  the  object  highlighted  in  Figure  10,  could  be  located  anywhere  within  the 
fifteen-degree  spread.  This  is  the  ambiguity  of  the  sonar  in  the  vertical  plane.  The  object 
shown  in  Figure  10  is  approximately  thirty  meters  in  front  of  the  vehicle.  Based  on  its 
range  and  the  bearing  ambiguity  of  the  FES,  the  object  can  be  located  within  +/-  4  meters 
in  altitude.  The  P450-15E  provides  a  range  resolution  of  2  inches  and  a  bearing 
resolution  in  the  image  plane  of  1  degree.  This  results  in  the  ensonified  volume  being 
divided  into  cube  like  shapes  with  dimensions  of  2  inches  by  1  degree  by  15  degrees  (see 
Figure  1 1). 
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Submersed  Rock 


Figure  10.  Submerged  Rock  in  a  Florizontal  FLS  Image 


Figure  1 1 .  Volume  segments  created  by  P450- 1 5E 


The  vertical  array  is  composed  of  a  single  stave  mounted  perpendicular  to  the 
horizontal  array.  Mounting  the  array  in  this  fashion  provides  angular  resolution  in  the 
vertical  direction.  Due  to  the  rotation  of  the  array,  the  ambiguity  of  the  sonar  shifts  to  the 
horizontal  direction.  All  vertical  sonar  images  will  have  fifteen  degrees  of  ambiguity  in 
the  horizontal  direction.  The  vertical  sonar  will  be  utilized  to  provide  the  location  of  the 
ocean  floor  and  the  height  of  the  objects  encountered.  A  typical  vertical  sonar  image  is 
displayed  in  Figure  12. 
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River  Surface 


Figure  12.  Vertical  Sonar  Image  depicting  the  river  surface  and  floor 


By  combining  the  separate  fields  of  view,  the  two  sonars  can  effectively  create  a 
volume  with  resolution  in  all  three  planes.  The  volume  ensonified  by  the  combination  of 
sonars  is  depicted  in  Figure  13.  Of  particular  importance  is  the  fifteen-degree  by  fifteen- 
degree  area  where  the  two  arrays  overlap.  This  overlapping  of  the  arrays  divides  that 
volume  of  water  into  sections  that  are  two  inches  in  depth  (vertically  and  horizontally)  by 
one  degree  in  vertical  width  and  one  degree  in  horizontal  width  (Figure  14). 
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Figure  13.  Volume  ensonified  by  the  combination  of  Vertical  and  Horizontal  Arrays 


Figure  14.  Volume  segments  created  by  the  overlapping  sonar 


The  vertical  and  horizontal  sonar  are  independent  sonar  systems.  Information  is 
not  shared  between  them.  Separate  two-dimensional  representations  are  created  by  each 
system.  The  horizontal  system  provides  resolution  in  the  horizontal  plane,  but  is  unable 
to  provide  resolution  in  the  vertical  plane.  The  vertical  system,  though  unable  to  provide 
resolution  in  the  horizontal,  provides  resolution  in  the  vertical.  In  order  to  create  a  three- 
dimensional  model  the  two  separate  data  streams  need  to  be  combined.  The  following 
chapters  will  discuss  the  use  of  occupancy  grids  to  combine  the  separate  systems  into  a 
single  three-dimensional  model. 
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D.  FLS  PROBABILITY  MODELS 


Knowing  that  the  purpose  of  the  FLS  sonar  is  to  populate  an  occupancy  grid 
necessitates  the  development  of  probabilistic  models  of  the  FLS.  For  a  sensor  to  be  used 
in  an  occupancy  grid,  two  separate  probabilistic  models  need  to  be  generated.  One  model 
describes  the  probability  of  receiving  a  value  when  the  return  can  be  attributed  solely  to 
noise.  The  second  model  describes  the  probability  of  receiving  a  particular  value  when 
the  sonar  return  can  be  attributed  to  a  physical  object.  This  section  will  focus  on  the 
development  of  the  probability  models  and  the  details  of  an  occupancy  grid  will  be 
discussed  in  the  next  chapter. 

1.  Horizontal  FLS  Noise  Model 

There  are  many  possible  sources  of  noise  for  the  sonar  system.  Like  all  electrical 
systems,  a  certain  amount  of  noise  can  be  attributed  to  the  various  types  of  electrical 
noise:  power  supply  fluctuation,  thermal  noise,  etc.  A  secondary  source  of  noise  is 
acoustic.  Acoustic  noise  includes  weather,  biological  sources,  or  other  vehicles  in  the 
area.  Acoustic  Models  have  been  created  for  the  various  types  of  noise,  but  these  models 
do  not  take  into  account  the  method  of  beamforming  used  in  blazed  array  sonar.  The 
final  category  of  noise  can  be  attributed  to  REMUS  itself.  REMUS  utilizes  several  other 
sensors  that  also  project  noise  into  the  water  column.  These  sensors  include  but  are  not 
limited  to  the  acoustic  modem,  ADCP  and  Side  Scan  sonar. 

Instead  of  designing  and  incorporating  theoretical  models  for  each  of  the  noise 
sources  an  experimental  approach  was  taken  to  create  a  comprehensive  noise  model. 
Within  the  dataset  used,  there  are  approximately  300  images  for  the  horizontal  FLS  that 
show  little  to  no  objects  in  the  field  of  view.  These  images  were  acquired  with  the 
vehicle  in  motion  and  in  the  same  acoustic  environment.  By  using  images  taken  in  the 
same  acoustic  environment,  the  noise  can  be  classified  as  a  single  distribution. 

The  expected  distribution  of  the  noise  will  be  a  combination  of  multiple  noise 
sources.  Because  the  resulting  distribution  of  the  noise  is  unknown,  a  histogram  was 
used  to  observe  the  distribution.  By  using  a  histogram,  an  initial  assumption  of  the 
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distribution  does  not  need  to  be  made.  When  creating  a  histogram,  the  bins  size  and 
spacing  can  play  an  important  role  is  determining  the  distribution. 

The  number  of  bins  will  be  detennined  by  the  range  of  values  expected.  Each 
image  in  the  data  set  is  a  sixteen  bit  image,  so  each  pixel  has  a  potential  range  of  value 
from  zero  to  65535.  However,  prior  experience  has  shown  that  the  maximum  expected 
value  of  noise  is  much  lower.  In  detennining  the  bin  size  and  spacing  a  subset  of  50 
images  were  analyzed.  The  combined  histogram  of  each  of  the  images  was  plotted.  In 
these  fifty  images,  the  maximum  value  of  noise  was  3958.  Based  on  the  maximum  noise 
value  and  considering  the  computer  storage  limitations  the  noise  histogram  contained  25 
bins  with  a  spacing  of  160  (with  the  exception  of  the  first  and  last  bin).  The  first  bin 
contained  values  from  zero  to  80;  the  last  bin  contained  values  from  3920  to  65536. 

With  the  bin  size  detennined,  the  images  were  analyzed  in  more  detail.  Due  to 
the  nature  of  the  FLS,  the  noise  distribution  varied  based  on  the  range  and  bearing  of  the 
pixel.  As  shown  in  Figure  15  the  distribution  cannot  be  readily  classified.  The 
distribution  of  noise  in  the  larger  ranges  appears  to  be  a  Gaussian  distribution,  where  the 
distribution  at  close  range  is  a  combination  of  distributions.  This  more  complex 
probability  distribution  function  may  be  attributed  to  the  near  field  effect  of  active  sonar. 
To  take  into  account  the  variability  of  the  distributions  across  the  entire  image,  a 
distribution  was  defined  for  each  pixel  in  the  image. 
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Figure  15.  Noise  Distribution  Changes  over  Range  (meters)  and  Bearing  (degrees) 


2.  Horizontal  FLS  Detection  Probabilities 

The  other  distribution  required  for  the  occupancy  grid  is  the  probability  of 
receiving  a  particular  value  given  that  the  value  is  attributed  to  a  return  from  an  object. 
Ideall,  this  model  would  be  based  on  a  theoretical  approach.  Theoretical  models  exist  for 
determining  sound  propagation  in  the  water.  Using  the  models,  the  intensity  level  of 
sound  can  be  predicted  at  any  point  in  the  water  column.  However,  the  algorithms 
utilized  by  BlueView  in  mapping  the  broadband  signal  received  at  the  FLS  stave  into  an 
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image  are  proprietary.  Without  knowledge  of  the  exact  algorithms  involved  in  generating 
the  FLS  images,  an  experimental  approximation  must  be  made. 

A  single  experiment  has  not  been  conducted  to  determine  the  distribution  of 
expected  received  values  at  every  pixel  in  the  image.  To  determine  the  distribution  of 
received  FLS  image  values  when  an  object  is  present  a  subset  of  the  Charles  River 
dataset  was  analyzed.  Unfortunately,  every  image  in  the  Charles  River  dataset  is  subject 
to  all  the  noise  sources  previously  discussed.  The  noise  distribution  must  be  separated 
from  the  signal  distribution. 


The  subset  of  images  used  for  this  portion  of  the  experiment  was  chosen  due  to 
the  large  area  that  the  objects  fdled  in  the  field  of  view.  In  a  similar  fashion  to  the  noise 
distribution,  the  histogram  of  each  pixel  was  determined.  The  following  process  was 
used  to  separate  the  detection  distribution  from  the  noise  distribution,  and  is  illustrated  in 
Figure  16  .  This  procedure  is  done  for  each  pixel  in  the  image. 


a.  Noise  Distribution  b.  Normalized  Noise  Distribution,  n 


-100  0  100  200  300  0  100  200 


c.  Detection  Distribution  d.  Normalized  Detection  Distribution, 


s 


e.  s-n  f.  (s-n)  raised  to  a  min  of  zero 
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g.  Adjusted  Detection  Distrobution  h.  Adjusted  Detection  +  Noise 
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Figure  16.  Determining  the  Probability  of  Detection  for  pixel  (50,  400)  of  the 

Florizontal  FLS 
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1.  Determine  P[pixel  value  |  no  Object]  (Figure  16a).  This  is  a  histogram  of 
the  raw  data  retrieved  from  the  dataset. 

2.  Normalize  /’[pixel  value  |  no  Object]  (Figure  16b).  In  this  step  the 
histogram  is  normalized  so  that  the  sum  of  all  of  the  bins  equal  one. 

3.  Determine  /’[pixel  value]  (Figure  16c).  The  raw  histogram  of  the  dataset 
that  contains  images  with  a  combination  of  objects. 

4.  Determine  /’[pixel  value  |  Object] .  The  probability  of  receiving  any  pixel 
value  is  a  summation  of  multiple  probabilities  (Equation  3-5). 

P [pixel  value]  =  P [pixel  value  |  Object]P[Object]  +  P [pixel  value  |  no  Object] P [no  Object]  3-5 


Rearranging  Equation  3-5  yields  Equation  3-6. 


r  .  ,  ,  ,  .  idObjectl  1 

idpixel  value  Object] — r  L — - J  ,  =  — = - ^ 

L  1  _P[no  Object]  P [no  Object] 


P [pixel  value]  -P [pixel  value  j  no  Object] 


3-6 


5. 

6. 


7. 


8. 


/’[no  Object]  was  estimated  by  the  ratio  of  the  sum  of  histogram  bins  of 
/’[pixel  values]  that  have  a  corresponding  non  zero  bin  in 
/’[pixel  values|no  Object] .  Figure  16d 
1 


shows 


/’[no  Object] 


/’[pixel  values] . 


P  [pixel  value|Object] 


P  [Object] 
P[no  Object] 


is  shown  in  Figure  16e. 


P  [Object] 


remain 


Ensure  that  all  values  of  P [pixel  value! Object]-  r 

L  JP  [no  Object] 

positive  (Figure  16f).  If  any  value  in  the  histogram  generated  in  step  five 
is  negative,  raise  all  values  of  the  histogram  by  the  maximum  negative 
value.  This  ensures  that  all  probability  values  are  non-negative. 

P  [Object] 


Determine  P [pixel  values  |  Object]  (Figure  16g).  Assume 
is  a  constant  and  normalize  P  [pixel  value|Object] 


P[no  Object] 
P [Object] 


This 


P[no  Object] 

provides  P[ pixel  values  |  Object] ,  the  final  distribution  used  to  model  the 
probabilities  of  receiving  a  value  given  that  an  object  is  present. 

Ensure  P[pixel  values  |  no  Object]  +  P[pixel  values  |  Object]  maintains  the 

relationship  of  the  original  distribution  (Figure  16h).  To  verify  that  the 
separate  distributions  are  accurate,  they  are  summed  and  normalized.  The 
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new  normalized  combination  distribution  is  compared  to  the  original 
nonnalized  distribution.  As  can  be  seen  in  Figure  16d  and  h,  the  resulting 
shape  of  the  distribution  is  similar  to  the  original. 

3.  Vertical  FLS  Model 

The  Vertical  FLS  is  comprised  of  staves  identical  to  those  used  in  the  horizontal 
FLS.  The  difference  between  the  vertical  and  horizontal  FLS  is  the  number  of  staves 
used  and  the  orientation  in  which  they  are  mounted.  The  vertical  FLS  covers  45  degrees 
in  the  field  of  view  (2  staves)  and  the  horizontal  FLS  covers  90  degrees  (4  staves).  The 
central  45  degrees  of  the  horizontal  sensor  distributions  should  be  identical  to  the 
distribution  of  the  vertical  FLS. 

4.  Verification  of  the  Probability  Models 

The  models  are  created  on  a  pixel-by-pixel  basis;  therefore  the  horizontal  model  is 
comprised  of  almost  two  million  individual  probability  functions  (461x2048x2).  With 
that  number  of  distributions  it  was  impractical  to  inspect  each  distribution.  To  verify  that 
the  relationship  between  the  noise  and  detection  distributions  were  valid,  a  sample 
Bayesian  probability  image  was  created  for  each  sensor. 

P(l  |  all  pixels  are  due  to  detections) 

Bayesian  Image  =  — - - - - - - - - -  ' 

P(l  |  all  pixels  are  due  to  detections)  +  P(/ |  all  pixels  are  due  to  noise) 

If  the  distributions  are  correct  then  the  Bayesian  image  will  show  a  high  intensity 
value  for  any  obstacle  in  the  image  and  will  show  a  very  low  intensity  in  areas  due  to 
noise.  Figures  17  and  18  show  the  original  FLS  image  with  the  resulting  Bayesian 
image.  As  can  be  seen  in  both  figures,  the  amount  of  noise  is  drastically  reduced  and  the 
objects  detected  are  greatly  enhanced.  The  clarity  of  the  images  proves  the  validity  of 
both  the  noise  and  detection  distributions.  As  can  be  seen  Figure  17,  further  future  work 
can  be  done  to  remove  the  noise  in  the  lower  portions  of  the  image. 
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Figure  17.  Horizontal  FLS  Image  (upper)  and  corresponding  Bayesian  Image  (lower) 


Figure  18.  Vertical  FLS  Image  (left)  and  corresponding  Bayesian  Image  (right) 

Both  images  are  in  polar  coordinates 
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E. 


GEO-LOCATING  IMAGE  DATA 


Up  to  this  point,  all  of  the  information  provided  by  the  FLS  has  been  referenced  to 
the  vehicle.  To  be  truly  useful  in  a  mapping  algorithm,  the  FLS  data  will  need  to  be 
mapped  into  the  global  space.  By  combining  the  vehicle  position  estimation  model  with 
the  FLS  model,  each  data  point  in  a  sonar  image  can  be  geo-located.  This  section  will 
cover  the  equations  necessary  to  couple  the  models  and  convert  sonar  images  into  to  map 
coordinates. 


1.  Development  of  the  Transformation  Equations 

The  height  of  the  image  corresponds  to  a  range  in  front  of  the  vehicle  (0  to  90m) 
and  the  width  of  the  image  corresponds  to  the  bearing  from  the  vehicle.  The  horizontal 
images  are  461  x  2048  pixels.  The  vertical  images  are  461  x  1024  pixels.  Both  of  these 
are  using  polar  coordinates  In  the  horizontal  images,  with  the  four  staves,  the  bearing  is 
the  spread  in  the  horizontal  plane  of  the  vehicle  and  covers  -45°  to  45°.  The  vertical 
images  cover  from  -22°  to  22°  in  the  vertical  plane  of  the  vehicle  (There  are  two  staves 
and  the  coverage  is  a  total  of  45  degrees).  Equation  3-8  shows  the  conversion  from  pixel 
indexes  into  the  range  and  bearing  from  the  vehicle. 


Horizontal  Image 

. .  Max  Range 

r  =  i* - 5 — 

Image  Height 


90° 

0n  =  - 45 0  +  j  * - — - 

Image  Width 


Vertical  Image 

.  .  Max  Range 

r  =  i* - 5 — 

Image  Height 


Vp 


-12°  +j* 


24° 

Image  Width 


where 

rp  :  Range  in  front  of  the  vehicle 
6  :  rotation  about  the  z  axis  in  the  XY  plane 
y/  :  rotation  about  the  x  axis  in  the  YZ  plane 
(/,/)  :  Pixel  location  (height,  width) 


3-8 


To  fully  describe  the  volume  ensonified,  Equation  3-8  needs  to  be  revised  to 


include  the  ambiguity  of  the  FLS.  At  this  point,  a  decision  needs  to  be  made  to  determine 
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the  resolution  associated  with  the  ambiguity  of  the  FLS.  Since  the  overall  goal  of  the 
system  is  to  combine  both  sensors  into  a  single  coherent  sensor  model,  the  resolution  of 
the  ambiguity  is  chosen  to  match  the  resolution  of  the  opposing  sensor.  This  provides  a 
one-degree  spacing  in  the  ambiguity  of  the  sonar  images  and  causes  9P  and  (pp  to  become 
vector  quantities. 


Horizontal  Image  Vertical  Image 

90  m 


•  *  90m 

r=i * - 

p  461 


0=-45°+j 


90 0 
2048 


r  =i* - 

p  461 


&p  ~ {-7.-6,. ..7} 


Yp  ~  {-7,-6,. ..7}  y/p  =  -12°  +  j 


24° 

1024 


3-9 


Combining  the  knowledge  of  the  vehicles  position  and  orientation  with 
Equation3-9,  allows  for  each  pixel  in  the  image  space  to  be  geo-located.  Define  the 
sensor’s  location  as  (x0,  y0,  z0)  with  an  orientation  of  (9V,  cpv,  '|N  ).  Due  to  the  small 
amount  of  roll  associated  with  the  vehicle  it  will  not  be  accounted  for  in  the 
transformation. 


(xe ,  y0 ,  zo )  :  Vehicle  postion  in  local  reference  frame 
Gv  :  Vehicle  Heading  (after  conversion  into  the  local  frame) 

(pv:  Vehicle  Pitch 
i//r:  Vehicle  Roll 

The  data  provided  by  a  single  pixel  can  then  be  geo-located  using  Equation  3-11. 

Xp  =  X  v  +  rp  cos [ev  +  6p ) cos  ($,  +  (j)p ) 
yp  =  Tv  +  rp  sin(^  +  °r, ) cos(</>v  +<f>p) 

ZP  =zv+rp  sin(^v  +^) 

Note:  d>  ,9  ,x  ,  v  ,z  are  vectors 

Equation  3-11  will  map  any  data  stored  in  the  image  space  into  the  global  space, 
whether  the  data  is  the  original  sonar  image  or  a  processed  version.  This  fact  will  be 
utilized  in  the  next  section  during  the  creation  of  the  occupancy  grids.  Since  the 
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occupancy  grids  use  a  probabilistic  approach,  each  image  will  be  transformed  from  the 
original  image  into  the  probabilistic  image.  The  probabilities  assigned  to  each  pixel  will 
then  be  mapped  into  the  occupancy  grid.  The  next  chapter  will  utilize  both  the  sonar  and 
vehicle  models  to  generate  the  occupancy  grid. 
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IV.  OCCUPANCY  GRIDS 


A.  MAP  CHOICE 

The  main  goal  of  providing  the  UUV  with  a  three-dimensional  map  is  safe  and 
accurate  navigation.  A  safe  route  is  one  in  which  the  vehicle  has  a  low  probability  of 
collision.  In  other  words,  the  vehicle  is  able  to  locate  and  avoid  the  obstacles  in  its  path. 
The  accurate  route  is  one  in  which  the  vehicle  is  able  to  successfully  move  from  a  starting 
position  and  reach  its  end  position.  This  thesis  will  not  delve  into  the  intricacies  of  route 
planning,  but  route  planning  defines  the  type  of  map  required  for  safe  navigation  and  thus 
must  be  considered.  These  goals  provide  the  requirements  for  the  map.  First,  the  vehicle 
needs  to  know  location  of  obstacles.  Second,  the  vehicle  needs  to  know  the  location  of 
free  space. 

The  most  intuitive  method  to  create  the  map  is  feature  recognition.  In  feature 
recognition  every  object  detected  by  the  sensor  is  classified.  The  object’s  shape, 
dimensions  and  location  are  then  stored  in  a  database.  Once  a  new  sensor  reading  is 
received,  it  is  correlated  to  all  the  objects  currently  stored  in  the  database.  Previously 
detected  objects  are  verified  and  the  new  objects  are  added  into  the  database. 

Feature  recognition  is  easily  grasped  conceptually,  but  can  be  complicated  to 
implement.  In  man-made  areas,  such  as  buildings,  the  range  of  shapes  required  to 
accurately  depict  the  environment  is  limited,  most  areas  can  be  approximated  by  simple 
shapes.  In  more  natural  settings,  the  variation  in  objects  requires  more  complex  shapes 
be  defined.  The  larger  dataset  of  shapes  added  to  the  algorithm  and  the  increased 
complexity  of  the  shapes  requires  large  amount  of  computations.  This  in  turn  requires  a 
very  large  amount  of  processing  power. 

With  feature  recognition,  every  shape  must  be  classified,  compared  against  the 
database  of  shapes,  and  stored  for  latter  comparison.  Cluttered  environments  will  require 
a  significant  number  of  classifications  and  comparisons.  As  the  vehicle  finds  more 
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objects,  more  processing  power  and  time  will  be  required.  The  variability  in  processing 
power  requirements  makes  feature  recognition  difficult  to  implement  on  small  scale 
UUVs. 

Feature  recognition  also  provides  much  finer  detail  than  is  required  by  the  UUV 
for  safe  navigation.  For  safe  navigation,  the  vehicle  only  needs  to  know  that  an  object 
exists  as  a  given  position.  It  does  not  matter  whether  that  object  is  cubic,  cylindrical,  or 
spherical.  It  only  matters  that  the  space  is  occupied.  For  that  reason,  occupancy  grid 
mapping  is  a  better  choice. 

B.  OCCUPANCY  GRIDS 

Instead  of  recognizing  and  organizing  objects  by  their  shape  and  location, 
occupancy  grids  divide  the  area  into  a  grid.  The  region  to  be  mapped  is  defined  and  then 
tessellated.  Each  element,  or  cell,  in  the  grid  is  assigned  a  value  indicating  whether  the 
cell  is  occupied.  This  idea  is  easily  extended  from  two-dimensional  (checkerboard)  to 
three-dimensions  (stacks  of  cubes).  Since  the  state  of  the  cell  could  only  take  on  one  of 
two  values.  The  cell  is  either  empty  or  occupied.  Since  the  cell  can  only  be  in  one  of  two 
states,  the  probability  of  the  cell  being  empty  plus  the  probability  of  the  cell  being 
occupied  must  equal  one  (Equation  4-1). 


P  s  (C)  =  £7;//;(y]  +  P  s (C)  =  Occupied ]  =  1 


4-1 


The  information  provided  by  the  sensors  does  not  directly  map  to  the  state  of  the 
cell.  The  state  of  the  cell  must  be  inferred  from  the  sensor  values.  Because  of  this 
inference,  the  state  of  the  cell  is  approximated.  In  an  ideal  world,  the  cell  could  only  take 
on  one  of  two  values.  The  cell  would  either  be  empty  or  would  be  occupied.  However, 
in  practice,  the  sensors  cannot  provide  the  definitive  state  of  the  cell.  Therefore,  the 
probability  of  the  state  of  each  cell  is  recorded.  The  determination  of  the  probability  of 
the  state  of  the  cell  given  the  current  reading  of  the  sensor  is  found  using  Bayes  theorem 
Devore[14]  defines  Bayes  Theorem  in  the  following  manner,  ‘Let  Ai,  A2,  ...At  be  a 
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collection  of  k  mutually  exclusive  and  exhaustive  events  with  prior  probabilities 
P(Ai)(i=l,...,k).  Then  for  any  event  B  for  which  P(B)>0,  the  posterior  probability  of  A, 
given  that  B  has  occurred  is 


At  Is) 


p(Aj  ns) 

P(B) 


±p(b\a,).p(aj) 


4-2 


1=1 

Utilizing  Bayes  theorem  will  provide  the  probability  of  the  state  of  the  cell  based 
on  the  current  measurements  only.  What  is  needed  is  the  state  of  the  cell  based  on  the 
current  and  all  previous  measurements.  Elfes  [16]  provides  an  iterative  solution  to 
determining  the  probability  that  a  cell  is  occupied  given  all  previous  measurements 
(Equation  4-3). 


pp(c)=occiww;  = 


Pfi;,,  |  s(C)  =  Occ]*p[s(C)  =  Occ  |  {r}  _ 

Vs(C) 


rt+1 :  The  current  measurement 
s(C):  State  of  the  cell 
{ r} :  All  measurement  up  to  time  t 


4-3 


Expanding  the  summation  in  the  probability  update  equation  yields  Equation  4-4. 
From  here,  it  is  easy  to  see  the  four  unknowns  that  must  be  determined  for  each  update. 
The  unknowns  are  listed  in  Table  7. 


P[i(C)  =  OcC|{r},+1] 


P\rM\s(C)  =  Occ]*P[s(C)  =  Occ\{r}^ 

/>[;;+l  | s(C)  =  Emptv^* P  s(C)  =  Empty \{r}t  +P[r(+1 1 s(C)  =  Occ]*P  s(C)  =  Occ\{r}t 


4-4 
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E[s(C)  =  Occ|{r}r+i] 

Probability  that  a  cell  is  occupied  given 
the  current  and  all  previous  measurements 

P[s(C)  =  0cc|{r}(] 

Probability  that  a  cell  is  occupied  given  all 
the  previous  measurements 

p[s(C)  =  Empty  \{r}t~\ 

Probability  that  a  cell  is  occupied  given  all 
the  previous  measurements 

p[r,+i  1  s(c)  =  °cc] 

Probability  of  receiving  the  current 
measurement  given  that  the  cell  is 
occupied 

p\jt+x  |  s(C)  =  Empty] 

Probability  of  receiving  the  current 
measurement  given  that  the  cell  is  empty 

Table  7.  Definition  of  Terms 


Since  the  cells  can  only  have  two  states,  empty  or  occupied, 
/’[5(C)  =  Empty  |  {r};  ]  can  be  substituted  with  1  -  /J^.s  (C)  =  Occ  |  {/'},]  ,  reducing  the 
number  of  values  to  be  calculated  for  each  cell.  For  example,  assume  a  cell  is  an 
unknown  state  P[^(C)  =  Occ]  =  0.6 and  a  new  measurement  is  received.  Based  on  the 
measurement  and  the  corresponding  probability  functions  the  values  of 
P\rt+l  |  s(C)  =  Occ]  and  P[r(+I  |  .s'(C)  =  Empty~^  are  determined.  Assume  that 

P\_rt+l  |  s(C)  =  Empty^  =  0.2  and  P\_rt+l  |  s(C)  =  Occ]  =  0.4 .  From  these  values  the 
probability  of  the  cell  being  occupied  is  updated  (Equation  4-5). 


p[,(c)=occiH+i; 


0.4*  0.6 

0.2  *(1-0.6) +  0.4  *0.6 


0.75 


4-5 


The  recursive  nature  of  this  problem  requires  a  seed  value  for 
/’[5(C)  =  Occ  |  jr}(  ]  .  At  time  zero,  or  the  time  prior  to  any  measurements,  this  value 

indicates  all  the  information  the  vehicle  has  about  its  environment.  When  the  vehicle  has 
zero  information  about  its  environment  the  cells  are  initialized  to  0.5.  An  initial  value  of 
0.5  indicates  that  the  cells  are  equally  likely  to  be  occupied  or  empty.  As  more 
information  is  attained  this  value  is  updated.  This  value  will  constantly  be  evolving 
based  on  the  current  input  from  the  sensor. 
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The  next  values  needed  are  the  probability  of  receiving  a  particular  measurement 
value  given  the  state  of  the  cell.  These  probability  distributions  are  individual  to  the  type 
of  sensor  employed.  Since  the  probability  distribution  is  dependent  on  the  sensor  type, 
the  distributions  should  be  defined  prior  to  vehicle  operation.  The  probability 
distributions  associated  with  the  sensor  can  be  determined  either  theoretically  or 
experimentally.  Whether  distributions  are  derived  theoretically  or  through 
experimentation,  their  values  can  be  stored  and  easily  referenced  at  run  time.  The  sensor 
probability  distributions  for  the  FLS  were  derived  experimentally.  The  methods  used  to 
derive  them  will  be  covered  in  Chapter  IV. 

The  typical  occupancy  grid  update  starts  when  a  new  measurement  is  received. 
The  sensor  location  is  correlated  to  an  occupancy  grid  location.  The  current  occupancy 
grid  value  is  retrieved.  Next,  the  measurement  is  converted  into  two  separate 
probabilities,  the  probability  of  receiving  the  signal  assuming  that  the  location  is  empty 
and  the  probability  of  receiving  the  signal  assuming  the  location  is  occupied.  Utilizing 
Equation  4-4  the  new  value  of  the  probability  of  the  grid  cell  being  occupied  is  calculated 
and  stored.  This  process  will  repeat  for  every  sensor  measurement  that  is  received. 

C.  COMBINING  MULTIPLE  SENSORS 

The  previous  discussion  only  covered  the  case  in  which  the  grid  was  dependent 
on  a  single  sensor.  However,  in  most  robotic  applications,  the  vehicle  will  have  multiple 
sensors.  The  vehicle  may  have  many  sensors  of  the  same  type  or  a  multitude  of  different 
classes  of  sensors.  It  is  through  the  combination  of  multiple  sensors  that  a  better 
understanding  of  the  environment  can  be  made.  In  order  to  take  full  advantage  of  all  the 
sensors  that  the  vehicle  deploys,  the  occupancy  grid  map  must  account  for  all  sensor 
values. 

The  most  accurate  method  of  combination  would  be  to  use  a  superbayesian 
approach.  In  this  method,  all  sensors  probabilities  would  need  to  include  the  dependency 
on  the  other  sensors  utilized.  For  example,  when  utilizing  two  different  sensors,  Si  and 
S2,  the  probabilistic  sensor  model  for  Slmust  also  take  into  account  any  information 
provided  by  S2.  The  new  probabilistic  model  for  Si  would  be  the  probability  of 
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receiving  the  particular  sensor  value  given  that  the  cell  is  occupied  and  that  S2  received  a 
given  sensor  value.  The  same  holds  true  for  S2.  This  redefines  the  probabilistic  model 
used  for  each  sensor  utilized. 

There  are  two  distinct  disadvantages  associated  with  the  method.  Many 
unmanned  vehicles  are  capable  of  changing  the  sensors  they  employ.  A  vehicle  that  has 
five  different  sensors  would  require  120  (5!)  sensor  models  to  account  for  all  of  the 
possible  sensor  configurations.  This  alone  makes  this  type  of  implementation 
impractical.  Assuming  that  all  120  sensor  models  are  created,  if  any  sensor  is  added  or 
upgraded  all  the  previous  sensor  models  are  superseded.  On  today’s  vehicles  that  are 
capable  of  rapid  changes  in  sensor  configuration,  this  method  is  not  practical. 

An  alternative  method  is  to  maintain  separate  occupancy  grids  for  each  sensor  and 
then  combine  the  grids  utilizing  an  independent  opinion  pool.  This  method  has  proven 
successful  by  Elfes  in  [15].  In  this  scenario,  each  sensor  will  generate  its  own  occupancy 
grid  values.  Assume  the  vehicle  is  utilizing  sensors,  SI  and  S2,  which  in  turn  generate 
occupancy  grids  PI  and  P2.  The  grid  probabilities  can  be  combined,  as  shown  in 
Equation  4-6. 

PP 

Pooled  Probability  = - 7 — 1  2.  . - -  4-6 

W+(l_/;)(l_^) 

The  individual  occupancy  grid  values  will  be  dependent  only  on  the  sensor  input 
and  its  probabilistic  model.  Maintaining  the  independence  between  sensors  allows  for  a 
singular  sensor  models  to  be  created.  For  example,  REMUS  is  also  equipped  with  a  side 
scan  sonar.  Defining  independent  models  for  side  scan  sonar  and  FLS  allows  for  their 
inclusion  or  exclusion  in  the  reconstruction  process  based  on  current  task.  For  the 
previous  vehicle  with  five  sensor,  this  reduces  the  number  of  required  sensor  models 
from  120  to  5.  With  the  fewer  number  of  models  required,  it  becomes  practical  to 
implement  this  algorithm  on  unmanned  vehicles. 

D.  SUMMARY 

Occupancy  grids  can  organize  multiple  sensor  inputs  into  a  fonnat  that  can  be 
used  for  safe  and  accurate  navigation.  They  do  not  require  any  prior  knowledge  of  the 
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area  and  can  easily  be  utilized  in  highly  congested  areas.  Occupancy  grids  are  also  well 
suited  to  combine  multiple  sensor  inputs.  The  ability  to  navigate  without  any  prior 
knowledge  and  the  ability  to  determine  the  environment  from  all  sensor  inputs  is  an  ideal 
combination  for  UUVs. 
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V.  RESULTS  AND  CONCLUSIONS 


A.  RESULTS 

Before  discussing  the  results,  it  is  important  to  recall  the  objectives  of  the 
research.  REMUS’s  ability  to  avoid  obstacles  is  dependent  upon  its  information  about 
the  environment.  Currently  REMUS  is  utilizing  two-dimensional  representation  of  the 
environment  for  path  planning  and  obstacle  avoidance.  This  limited  REMUS’s  ability  to 
navigate  successfully  in  cluttered  environments,  like  under  a  bridge. 

In  2007,  REMUS  was  deployed  in  the  Charles  River  and  sent  on  a  path 
underneath  the  Massachusetts  Avenue  Memory  Bridge.  Figure  19  is  a  picture  of  the 
Massachusetts  Avenue  Bridge.  Figure  20  shows  the  path  of  the  vehicle.  The  path  is 
shown  in  blue  and  the  red  circles  indicate  edges  of  the  pylons  supported  the  bridge.  The 
GPS  position  of  the  pylons  were  acquired  by  placing  a  handheld  GPS  unit  in  close 
proximity  to  the  part  of  the  pylons  that  were  above  the  surface.  A  limited  number  of 
satellites  were  acquired  while  receiving  the  position  due  to  the  bridge  occluding  GPS 
satellite  reception.  Due  to  the  limited  number  of  satellites  received,  there  may  be  errors 
associated  with  the  GPS  positions  of  the  columns. 


Figure  19.  Massachusetts  Avenue  Bridge,  Boston,  MA, 

from  [18] 
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Figure  20.  Vehicle  Path  in  the  Charles  River  and  under  the  Massachusetts  Avenue 

Bridge 


As  discussed  in  Chapter  I,  the  vehicle  would  not  be  able  to  navigate  successfully 
between  the  pylons  and  over  the  berm  if  it  relied  on  obstacle  avoidance  and  two- 
dimensional  representations  of  the  bridge.  This  can  be  seen  by  Figure  4.  With  the 
combination  of  horizontal  and  vertical  FLS  and  occupancy  grids,  a  path  emerges.  Figure 
21  shows  the  occupancy  grid  created.  The  grid  was  formed  using  eleven  images;  6 
horizontal  and  5  vertical.  These  were  the  total  number  of  images  available  to  the  vehicle 
as  it  approached  the  bridge.  Figure  21  is  composed  of  small  cubes.  A  solid  black  cube 
corresponds  to  a  probability  of  occupancy  of  nine-tenths  or  greater.  Cubes  with  a 
probability  between  five  and  nine  tenths  are  increasing  shades  of  red.  Cubes  with  a 
probability  of  one  half  or  less  are  not  depicted.  Cubes  with  values  of  one  half  or  less 
indicate  unknown  areas  or  areas  not  likely  to  be  occupied.  These  cubes  are  not 
displayed. 
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Comparing  Figure  21  with  Figure  4  the  success  of  occupancy  grids  readily  stands 
out.  In  the  horizontal  image  of  Figure  4,  the  four  berms  were  apparent.  The  problem 
with  the  horizontal  view  was  that  the  berm  appeared  as  a  solid  wall.  In  Figure  21,  the 
central  section  of  the  wall  created  by  the  hern  has  a  reduced  height.  This  section  is  the 
area  cnsonilied  by  the  vertical  FLS.  The  vertical  FLS  has  lowered  the  height  of  the  berm 
to  its  correct  value.  The  vehicle  can  now  recognize  the  path  through  the  berm.  A  similar 
effect  happens  with  the  space  between  the  pylons.  Due  to  the  ambiguity  of  the  vertical 
FLS,  the  columns  formed  a  vertical  wall  that  covered  the  entire  field  of  view.  However, 
once  combined  with  the  horizontal  FLS  information,  the  edges  and  dimensions  of  the 
columns  become  easily  identifiable. 


Figure  2 1 .  Occupancy  Grid  Developed  with  1 1  consecutive  images  (6  Horizontal,  5 

vertical)  with  the  vehicle  on  a  southwest  approach 


Successfully  locating  a  path  for  the  vehicle  is  just  part  of  the  problem;  the  vehicle 


must  also  be  able  to  correctly  place  the  path  into  the  global  space.  To  determine  the 
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accuracy  of  the  generated  map,  the  positions  of  the  generated  pylons  are  compared  to 
measured  values.  The  GPS  location  of  the  pylons  was  determined  with  a  hand  held  GPS 
unit.  Figure  22  shows  an  aerial  view  of  the  occupancy  grid  with  the  GPS  measurements 
overlaid  in  green  circles.  The  distance  between  the  GPS  position  and  the  closest  gridded 
pylon  location  were  calculated.  The  results  are  listed  in  Table  8. 
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Actual 

Cell  Location 

Distance  (m) 

(-294.1,315.2) 

(-296,  -315) 

1.9 

(-292.7,  -317.1) 

(-296,  -316) 

3.5 

(314.6,  -320) 

(-314.  -320) 

0.6 

(-288.6,  -350.4) 

(-289,  -350) 

0.6 

(-283.1,-350.4) 

(-284,  -349) 

1.7 

(-274.9,  -367.1) 

(-276.  -367) 

1.1 

(-294.1,-367.1) 

(-291,-369) 

3.6 

(-261.2,-389.3) 

(-264,  -397) 

8.2 

(-259.8,  -393) 

(-267,  -396) 

7.8 

(-279,  -293) 

(-273,  -298) 

7.8 

(-283.1,396.8) 

(-281,-401) 

4.7 

(-279,  -400.5) 

(-279,  -401) 

0.5 

Table  8.  Actual  Pylon  Location  versus  Cell  Location 


When  analyzing  this  data,  a  couple  of  observations  become  apparent.  There  is  a 
significant  offset  between  the  location  and  gridded  location  of  the  lower  pylon.  The 
possible  sources  of  error  will  be  discussed  in  the  following  section.  Excluding  the  lower 
most  pylon,  the  rest  of  the  data  shows  a  mean  distance  error  of  1.8  m.  This  error  is  much 
larger  than  expected  with  the  high-resolution  capabilities  of  the  FLS. 

B.  SOURCES  OF  ERROR 

Several  factors  play  a  role  in  the  overall  error  distance.  One  of  the  main 
contributing  factors  is  the  grid  size.  The  grid  size  (lm  x  lm  x  0.5m)  was  chosen  to 
reduce  the  number  of  computations  required.  The  grid  size  also  provided  enough 
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resolution  for  the  vehicle  to  determine  a  path  through  under  the  bridge.  However,  this 
large  grid  size  does  not  lend  itself  to  a  direct  relationship  with  the  sensor  resolution. 

Another  contributing  factor  is  the  method  used  for  pixel  probability  placement 
into  the  grid.  A  single  grid  cell  will  have  multiple  pixels  plotted  to  it  for  a  single  update. 
In  this  experiment,  the  highest  probability  values  were  placed  within  the  cell.  This 
method  disregards  information  provided  by  the  range  of  pixel  contained  in  the  cell. 

The  final  contributing  factor  to  the  error  is  the  vehicle  position  and  orientation  at 
the  time  of  the  image.  There  are  two  separate  contributors  to  this  one  source  of  error. 
First,  the  image  data  is  not  sampled  at  the  same  time  or  by  the  same  computer  as  the 
vehicle  state  information.  Because  of  this,  the  vehicle  state  information  was  interpolated 
to  determine  an  approximate  value  at  the  time  of  the  image.  Any  error  in  the  state 
information  is  propagated  through  the  algorithm  and  results  in  displaced  detections.  The 
second  contributor  to  errors  in  the  vehicle  state  is  the  approximations  made  by  the 
internal  navigation  unit.  The  vehicle  received  a  position  fix  at  the  beginning  of  the 
mission  but  was  unable  to  receive  any  more  fixes  for  the  duration  of  the  mission. 

C.  RECOMMENDATIONS 

The  research  has  demonstrated  the  proof  of  concept  of  utilizing  dual  FLS  with 
occupancy  grids  to  reconstruct  a  three-dimensional  environment.  However,  progress 
needs  to  be  made  before  the  process  is  deployable  on  a  vehicle  in  real  time.  Some  of  the 
areas  that  need  further  investigation  are  detailed  below. 

1.  Probabilistic  FLS  Model 

The  validity  of  the  model  created  for  this  experiment  is  limited  to  use  in  the 
Charles  River.  The  model  does  not  take  into  account  the  variability  of  many  of  the 
sources  of  noise.  Many  factors  can  affect  the  noise  distributions.  To  be  truly  useful  the 
noise  distribution  will  need  to  take  into  account  changes  in  the  vehicles  operational 
parameters.  For  example,  how  does  the  noise  change  as  the  speed  of  the  vehicle 
changes?  Does  flow  noise  contribute  significantly?  What  is  the  correlation  in  the  noise 
model  and  the  acoustic  noise  in  the  environment?  Many  parameters  need  to  be  analyzed 
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to  develop  a  robust  noise  model.  The  detection  distribution  utilized  for  the  FLS  model 
will  also  need  refinements.  It  is  recommended  that  specific  experiments  be  undertaken  to 
provide  a  better  classification  of  noise  and  detection  distributions. 

2.  Algorithm  Improvements 

The  algorithm  in  its  current  form  is  too  slow  to  implement  on  an  operational 
vehicle.  An  update  based  on  a  single  image  requires  approximately  60  seconds  to 
process.  This  is  unacceptable  for  real  time  operation.  It  is  recommended  that  the 
algorithm  be  transferred  from  a  Matlab,  a  prototype  environment,  to  a  language  designed 
for  speed.  One  possible  source  of  improvement  is  to  down  sample  the  FLS  images.  The 
current  images  provide  a  bearing  resolution  of  0.044  degrees.  The  actual  minimum 
resolution  of  the  FLS  is  specified  as  one  degree.  If  the  images  are  re-sampled  in  a 
manner  to  minimize  the  data  lost,  the  number  of  computations  can  be  drastically  reduced. 
One  possible  method  that  can  be  used  to  resample  the  images  is  to  use  a  Markov  Random 
Field  to  determine  the  relationship  among  the  pixels.  Once  a  viable  method  for 
resampling  has  been  found,  the  corresponding  distribution  functions  will  need  to  be 
created. 


3.  Combine  the  Occupancy  Grid  Route  Planning  Algorithm 

The  final  recommendation  is  to  couple  this  algorithm  with  route  planning.  The 
main  goal  of  this  thesis  is  to  provide  the  vehicle  a  method  to  determine  safe  routes  for 
navigation  in  a  cluttered  environment.  While  this  thesis  has  shown  a  feasible  method, 
until  it  is  coupled  with  a  route  planning  algorithm,  its  full  utility  cannot  be  detennined. 

D.  CONCLUSIONS 

There  are  issues  to  be  addressed  and  further  research  needs  to  be  conducted,  but 
this  research  can  stand  as  a  proof  of  concept.  It  is  possible  to  provide  UUVs  with  a 
reconstructed  three-dimensional  representation  of  the  underwater  environment.  This 
representation  can  then  be  used  for  route  planning  and  obstacle  avoidance.  Using  a 
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three-dimensional  reconstruction  provides  UUVs  with  the  capability  of  successfully 
navigating  in  cluttered  environments.  With  this  capability,  it  becomes  possible  to  safely 
deploy  UUVs  into  more  complex  environments. 
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APPENDIX 


MATLAB  CODE 


A.  MERGING  VEHICLE  AND  SONAR  IMAGE  DATASETS 

dataset  =  zeros (8100, 15) ; 

"o  "6 

%  dataset  =  [elapsed  time,  lat.  Ion,  x,  y,  depth,  altitude,  heading,  pitch,  . . . 
%  roll,  velocity,  headingRate,  Horizontal  Image  #,  Vertical 

Image  #] 

%  Original  Data  located  in  VehicleData,  VertData,  HorData 

%Vehicle  Elapsed  time  AtBeginning  of  first  turn:  736  sec 

%Sonar  Elapsed  time  at  Beginning  of  first  turn:  685  sec 

offset  =  736-685 

j  =  i; 

hdl  =  zeros (1,15)  ; 

hd2  =  hdl; 

vdl  =  zeros (1,15)  ; 

vd2  =  vdl; 

for  i  =  35:3000 

%Retrieve  time  of  Horizontal  FLS 
Htime  =  HorizData ( i , 1 ) ; 

%Retrieve  time  of  Vertical  FLS 
Vtime  =  VertData ( i, 1 ) ; 

%Get  Vehicle  State  Associated  with  Horizontal  FLS  and  Interpolate 
Hn  =  find (VehicleData (:, 9) < (Htime+off set) ,  1,  'last')-l; 
hdl  =  [VehicleData (Hn, 9) ,  VehicleData (Hn,  10:11),  0, 

0, VehicleData (Hn, 12:18)]; 

Hn  =  Hn+1 ; 

hd2  =  [VehicleData (Hn, 9) ,  VehicleData (Hn,  10:11),  0, 

0, VehicleData (Hn, 12:18)]; 

intp  =  (hd2 (1) - (Htime+off set) ) / (hd2 (1) -hdl (1) ) ; 

[Hn,  VehicleData (Hn-1 ,  9),  Htime+off set,  VehicleData  (Hn, 9) ,  intp] 
hd  =  hdl  +  intp* (hd2-hdl) ; 

%Get  Vehicle  State  Associated  with  Vertical  FLS  and  Interpolate 
Vn  =  find (VehicleData (:, 9) < (Vtime+off set) ,  1,  'last'); 
vdl  =  [VehicleData (Vn, 9) ,  VehicleData  (Vn,  10:11),  0, 

0, VehicleData (Vn, 12:18)]; 

Vn  =  Vn+1 ; 

vd2  =  [VehicleData (Vn, 9) ,  VehicleData (Vn,  10:11),  0, 

0, VehicleData (Vn, 12:18)]; 

intp  =  (vd2 (1) - (Htime+off set) ) / (vd2 (1) -vdl (1) ) 
vd2  =  vdl+intp* (vd2-vdl ) ; 

%Store  combined  data  to  a  new  dataset  ordered  by  offset  time 
switch  logical (true) 
case (Htime<=Vtime) 

dataset  (j,:)  =  [hd2,  i,  i-1,  0] ; 
dataset  (j+1,  :)  =  [vd2,  i,  i,  0]; 

j  =  j+2; 
case (Htime>Vtime) 

dataset  (j,:)  =  [vd2,  i-1,  i,  0]  ; 
dataset (j+1,  :)  =  [hd2,  i  ,  i,  0] ; 

j  =  j+2; 

end 

end 

%Convert  GPS  to  meters  in  a  local  grid 
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%  Reference  point  (42.358301,  -71.088055) 
for  i  =  1 : size (dataset, 1) 
lat  =  dataset  (i,  2)  ; 

Ion  =  dataset  (i, 3)  ; 

dataset  (i, 4 )  =  (lat-42 . 358301 )* 60*1853 . 47 ; 

dataset  (i, 5)  =  cos (2*pi*lat/360) *60*1853 . 47* (Ion— 71 . 088055) ; 

end 

clear  hdl  hd2  vdl  vd2  intp  i  Vn  Hn  Htime  Vtime  j  lat  Ion 
%  clear  VehicleData  VertData  HorizData 


B.  GENERATING  FLS  PROBABILITY  MODEL 

bins  =  linspace (0, 4000, 25) ; 
binstorage  =  zeros ( size (bins) ) ; 

%Generate  Noise  Distributions 

binstorage  =  zeros  (461,  2048,  numel (bins) ) ; 

for  i  =  300:20:600  %Range  of  Images  involved  in  Noise  Distribution 
%Combine  20  Images 
for  j  =  1:20 

imageData ( : , : ,  j)  =  OpenHorSonarlmage ( ' . . \SonarImages\8_bit ' 
'fls_horz_',  4,  '.png',  i+j-1) ; 

end 

m  =  zeros  (size (binstorage) ) ; 

%Create  Histograms 

for  k  =  1 : size ( imageData, 1 ) 

for  p  =  1 : size (imageData, 2) 

m(k,p, :)  =  hist (imageData (k,p, :) ,bins) ; 

end 


end 

binstorage  =  binstorage+m; 
i  =  i 

clear  imageData 


end 

clear  i  j  k  p  m  bl 

%Save  Histogram  as  Noise  Histogram 
Vnoise  =  binstorage; 

%Generete  Sensor  Histograms 
imageData  =  zeros (461,  2048,  20); 
binstorage  =  zeros  (461,  2048,  numel (bins) ) ; 

%Image  Ranges  used  in  Dataset 

I_range  =  [820,  960;  1125,  1265;  1505,  1617;  1840,  1910] ; 
for  1=1:4 

start  =  I  range  (1, 1) ; 
stop  =  I_range  (1, 2)  ; 
for  i  =  start : 20 : stop 
for  j  =  1:20 

imageData (:,: ,  j)  =  OpenHorSonarlmage ('.. \SonarImages\8_bit 
'fls_horz_',  4,  '.png',  it  j  )  ; 

end 

m  =  zeros (461,  2048,  numel (bins )) ; 
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end 


for  k  =  1 : size ( imageData, 1 ) 

for  p  =  1 : size (imageData, 2) 

m(k,p, :)  =  hist (imageData (k,p, :) ,bins) ; 

end 

end 

binstorage  =  binstorage+m; 
i  =  i 

end 

%Save  Sensor  Distribution 
Vsensor  =  binstorage; 

Vbins  =  bins; 

clear  imageData  binstorage  I_range  start  stop 
clear  i  j  m  k  p 


%Remove  Noise  From  Sensor  Distribution 
n  =  zeros (size (Vbins) ) ; 
s  =  n; 
t  =  n; 

i  =  200;  j  =  880; 

%Update  the  Distributions  for  Each  Pixel 
%  for  i  =  1:461 
%  for  j  =  1:2048 

%Retrieve  Noise  Histogram 
n(:)  =  Hnoise (i, j ,:) ; 

figure  (4);  subplot (4, 2, 1) ;  bar(Hbins,  n) ;  title  ('Noise 
Distribution ' ) 

%Normalize  Noise 
n  =  n/sum (n) ; 

[mn ,  mi ]  =  max (n) ; 

figure  (4);  subplot ( 4 , 2 , 2 ) ;  bar(Hbins,  n) ; title ( 'Normalized  Noise 
Distribution,  n') 

%  Retieve  Sensor  Histogram 
s(:)  =  Hsensor ( i,  j  ,  : )  ; 

figure (4);  subplot  ( 4 , 2 , 3) ;  bar(Hbins,  s) ;  title  (' Detection 
Distribution ' ) 

%  Normalize  sensor  over  range  of  Noise  Distribution 
s  =  s/sum (s .* (n>0) ) ; 

figure  (4);  subplot  ( 4 , 2 , 4 ) ;  bar(Hbins,  s) ;  title (' Normalized 
Detection  Distribution,  s  '  ) 

%Remove  Noise  and  Ensure  all  values  are  positive,  then  Normalize 
s  =  s-n; 

%  figure  (4);  subplot  ( 4 , 2 , 5) ;  bar(Hbins,  s) ;  title ('s-n') 

s=  s  +  abs (min (s. *  (s<0) ) )  ; 
s  =  s/ sum (s) ; 

figure  (4);  subplot ( 4 , 2 , 6) ;  bar(Hbins,  s) ;  title ('(s-n) 
raised  to  a  min  of  zero') 

%  s  =  s+0 . 001*abs (min (s+10 . * ( s==0) ) ) ; 

figure  (4);  subplot  ( 4 , 2 , 7 ) ;  bar(Hbins,  ( s+n) /sum (s+n) )  ; 
title ( 'Adjusted  Detection  Distrobution ' ) 

%  s  =  ( 1  —  s )  .* (s~=0)  ; 

s  =  s/ sum (s) ; 

figure  (4);  subplot  ( 4 , 2 , 8 ) ;  bar(Hbins,  ( s+n) /sum  ( s+n) ) ; 
title ( 'Adjusted  Detection  +  Noise') 

n  =  n+max (min (n+1 0 . * (n==0 ) ) , 0 . 01 ) ; 

Save  Adjusted  Distribution  to  Sensor  and  Noise  Models 
Hnoise ( i, j ,: )  =  n/sum(n); 

Hsensor  (i, j , : )  =s; 

end 


end 
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c 


GENERATING  THE  MAP 


1.  Main  Function 

%ESTABLISH  THE  BOUNDARIES  OF  THE  GRID 
b  =  [0  0.25  50;  0  .25  50;  0  0.25  10]; 
b  ( 1 , 1  :  3 )  =  b  ( 1 ,  1 : 3)  +  -300; 
b  (2 , 1  :  3 )  =  b ( 2 , 1 : 3)  +  -385; 
b  (1 : 2 ,  2)  =  0.25; 
for  i  =  1 : 3 

b(i,4)  =  numel (b  (i,  1)  :b  (i,  2) :b (i, 3) )  ; 

end 
b  =  b 

%Initialize  all  variables 
%Image  Containers 
imgH  =  zeros  (461,  2048); 
imgS  =  zeros ( size (imgH) ) ; 
imgV  =  zeros (461,  1024); 
imgSv  =  zeros ( size ( imgV) ) ; 

%Grid  Containters 

gH  =  0. 5*ones  (b  (1, 4) ,  b(2,4),  b(3,4)); 
gV  =  0 . 5*ones (size (gH) ) ; 
gN  =  zeros ( size (gH) ) ; 
gS  =  zeros ( size (gH) ) ; 
gM  =  0 . 5*ones (size (gH) ) ; 

%Vehicle  State 
vd  =  zeros  (1,6); 

HIN  =  0; 

VIN  =  0; 

dr  =  [1662,  1672  ;  2280,  2290  ]  %Establish  images  be  considered 
for  1  =1 

for  vdn  =  dr ( 1 , 1 ) : dr ( 1 , 2 ) 
tic 

%Extract  Vehicle  state  and  Images  Numbers 

vd  =  [dataset (vdn,  5),  dataset (vdn,  4),  dataset (vdn,  7),  90- 
dataset  (vdn,  8),  dataset (vdn,  9),  dataset (vdn,  10)]; 

%  Generate  Horizontal  Map 
if  HIN  <  dataset (vdn,  13) 

HIN  =  dataset (vdn,  13); 

imgH  =  OpenHorSonar Image ('.. /Sonarlmages/RTheta ' ,  ' fls_horiz_ ' , 

4,  '.pgm',  HIN);  %Get  Images 

imgH ( find (isnan (imgH) ) ) =2 ; 

imgS  =  Problmages (imgH,  Hsensor,  Hbins) ;  %Detection  Prob 

Image 

img  =  Problmages ( imgH,  Hnoise,  Hbins);  %Noise  Prob  Image 

gS  =  HorImage2Map (imgS,  vd,  b) ;  %Detection  Prob  Grid 

gMask  =  gS>=0;  %Mask  of  updated 

Values 

gN  =  HorImage2Map ( img,  vd,  b) ;  %Noise  Prob  Grid 

%Horizontal  Grid  Update 
gS  =  (gS . *gH) . / (gN. * ( 1-gH) +gH . *gS ) ; 
gH  =  gS . *gMask+gH . * (~gMask) ; 

end 


Generate  Vertical  Map 
if  VIN  <  dataset (vdn,  14) 
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VIN  =  dataset (vdn,  14); 

imgV  =  OpenVertSonarlmage ( ' . . \SonarImages\RTHeta ' ,  ' f ls_vert_ ' , 

4,  '.pgm',  VIN);  %Get  Image 

imgSv  =  Probimages ( imgV,  Vsensor,  Vbins);  %Detection  Prob  Image 

imgV  =  Probimages (imgV,  Vnoise,  Vbins) ;  %Noise  Prob  Image 

gS  =  VertImage2Map ( imgSv,  vd,  b) ;  %Detection  Prob  Grid 

gMask  =  gS>=0;  %Grid  Mask 

gN  =  VertImage2Map ( imgV,  vd,  b) ;  %Noise  Prob  Grid 

%  Vertical  Grid  Update 

gS  =  (gS . *gV) . / (gN. * ( 1-gV) +gS . *gV) ; 

gV  =  gS . *gMask+gV. * (~gMask) ; 

end 


%  Pooled  UPdate 

gM  =  (gV. *gH) . / (gH. *gV+ (1-gV) . * (1-gH) ) ; 

[HIN,  VIN,  vdn]  %Show  Loop  Progress 

end 

end 

2.  Converting  FLS  Images  to  Probabilities 

function  Prob  =  Probimages ( img,  SensorDist,  binSpacing) 

%This  function  creates  to  probability  images  based  upon  the 
provided  image 

%and  the  Sensor  Probability  Distribution  (modeled  as  a  histogram) . 
Each 

%bin  contains  the  probability  of  recieving  a  sensor  value  within 
the  edges 
%of  the  bin. 

"o 

%  %Inputs: 

%  img:  Sensor  Image  M  x  N 

%  binSpacing:  Vector  indicating  the  edges  of  the  probability 

bins  (length  P) 

%  SensorDist:  Probability  Distribution  of  the  sensor. 

%  M  x  N  x  P 

%  Outputs : 

%  Prob:  An  M  x  N  matrix  where  each  element  represents  the 

probability  of  receiving  each  pixel 

-6 

Prob  =  0 . 5*ones  ( size ( img) ) ; 

%Traverse  the  entire  image 
for  j  =  1 : size ( img, 2 ) 

for  i  =  1 : size ( img, 1 ) 

%Find  out  which  Bin  number  the  intensity  falls  into 
binNum  =  find (img ( i , j ) <binSpacing,  1,  'first'); 
if  binNum  >0 

%Retrieve  probability  of  receiving  that  intensity  value 

Prob(i,j)  =  squeeze (SensorDist (i, j , binNum) ) ; 
else 

Prob(i,j)  =  0.0001;  %Assign  a  minimum  Probability 

end 

end 

end 
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3.  Mapping  Image  Data  to  Grid  Locations 


a.  Horizontal  FLS 

function  imageMap  =  HorImage2Map ( img, VehicleData,  bounds) 


%  %bounds 

%  [ 

xmin. 

xres, 

xmax, 

# 

of 

cells 

"o 

ymin, 

yres. 

ymax. 

# 

of 

cells 

"o 

temp 
cnt  = 

zmin, 
=  []  ; 

1; 

zres, 

zmax, 

# 

of 

cells 

ambspacing  =  0.5*pi/180; 
ambend  =  7*pi/180; 

%Set  Vehicle  position  and  orientation  parameters 

xO  =  VehicleData ( 1 ) ;  %LocalX 

yO  =  VehicleData (2) ;  %LocalY 

zO  =  VehicleData (3) ;  %Depth 

heading  =  VehicleData ( 4 ) *pi/l 80 ; 

pitch  =  VehicleData (5) *pi/180; 

roll  =  VehicleData ( 6) *pi/180; 


%Gen  imageMap 

xr  =  bounds (1 , 1 ): bounds (1 , 2 ): bounds (1 , 3)  ; 
yr  =  bounds (2 , 1 )  : bounds (2 , 2 )  : bounds (2 , 3)  ; 
zr  =  bounds (3, 1 ): bounds (3, 2 ): bounds (3, 3) ; 

imageMap  =  -l*ones (bounds (1 , 4 ) ,  bounds (2, 4),  bounds (3, 4 ) ) ; 

%Establish  Image  Parameters 
imgH  =  size (img, 1); 
imgW  =  size (img, 2); 

%Spherical  Coordinates 

theta  =  linspace (pi/4 ,  -pi/4,  imgW) ; 

r  =  linspace (90, 0,  imgH); 

phi  =  -ambend : ambspacing : ambend; 

for  phil  =  1 : size (phi , 2 ) 

[gr,  gt,  gphi  ]  =  ndgrid(r,  theta,  phi (phil)); 

%Convert  data  from  Local  Spherical  to  Global  Cartesian 
[x,  y,  z]  =  sph2cart (gt+heading,  gphi+pitch,  gr)  ; 

%Move  Image  Data  to  Vehicle  Location 

x  =  x+xO; 
y  =  y+yO; 
z  =  z  +  zO ; 

%Covert  Image  Position  to  Grid  Values 

xi  =  ceil (  (x-bounds (1, 1) ) /bounds  (1,2) ) ; 
xi  =  ((xi>0)&(xi<size(xr,2))).*xi; 

yi  =  ceil (  (y-bounds ( 2 , 1 ) ) /bounds (2 , 2 )  )  ; 
yi  =  ( (yi>0) & (yi<size (yr, 2) -1 ) ) . *yi ; 
zi  =  ceil  (  (z-bounds (3, 1) ) /bounds (3, 2) ) ; 
zi  =  (  ( zi>0 ) & ( zi<size  ( zr , 2 ) -1 ) )  .  * zi ; 

%Store  Image  Values  into  Grid  Position 
for  r I  =  1 : imgH 

for  thetal  =  1 : imgW 
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%Determine  if  image  value  is  contained  within  map 
valid  =  (xi (  rl,  thetal,  1)>0  &  yi (  rl,  thetal,  1)>0  & 
zi  (  rl,  thetal,  1)>0); 

%If  Valid  Store  Data  contain  in  image  pixel  to 
corresponding  Grid 

%Locations 
if  valid 

val  =  imageMap(xi(  rl,  thetal,  1),  yi (  rl,  thetal, 
1),  zi  (  rl,  thetal,  1)); 

imageMap(xi(  rl,  thetal,  1),  yi (  rl,  thetal,  1), 
zi  (  rl,  thetal,  1))  =  max(img(rl,  thetal),  val); 
end 

end 

end 

end 

b.  Vertical  FLS 

function  imageMap  =  VertImage2Map (img, VehicleData,  bounds) 


%  %bounds 

%  [ 

xmin, 

xres, 

xmax 

-o 

ymin. 

yres. 

ymax 

"o 

temp 
cnt  = 

zmin, 
=  []  ; 

1; 

zres, 

zmax 

ambspacing  =  0.5*pi/180; 
ambend  =  7*pi/180; 

%Set  Vehicle  position  and  orientation  parameters 

xO  =  VehicleData ( 1 ) ;  %LocalX 

yO  =  VehicleData (2) ;  %LocalY 

zO  =  VehicleData (3) ;  %Depth 

heading  =  VehicleData ( 4 ) *pi/l 80 ; 

pitch  =  VehicleData (5) *pi/180; 

roll  =  VehicleData ( 6) *pi/180; 


%Gen  imageMap 

xr  =  bounds (1 , 1 ): bounds (1 , 2 ): bounds (1 , 3)  ; 
yr  =  bounds (2 , 1 ) : bounds (2 , 2 ) : bounds (2 , 3) ; 
zr  =  bounds (3, 1 ): bounds (3, 2 ): bounds (3, 3)  ; 
imageMap  =  -l*ones  (size  (xr, 2) ,  size(yr,2),  size(zr,2)); 

imgH  =  size (img, 1); 
imgW  =  size (img, 2); 

phi  =  linspace ( -pi/ 1 6 ,  pi/16,  imgW) ; 

r  =  linspace ( 90, 0,  imgH); 

theta  =  -ambend : ambspacing : ambend; 

for  thetal  =  1 : numel ( theta) 

[gr,  gt,  gphi  ]  =  ndgrid(r,  theta (thetal ) ,  phi) ; 
[x,y, z]  =  sph2cart (gt+heading,  gphi+pitch,  gr)  ; 

x  =  x+xO; 
y  =  y+yO; 
z  =  z+zO ; 

xi  =  ceil  (  (x-bounds (1, 1) ) /bounds (1,2) ) ; 
xi  =  ( (xi>0 ) & (xi<size  (xr , 2 ) ) )  . *xi ; 

yi  =  ceil  (  (y-bounds ( 2 , 1 )) /bounds (2,2)); 
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yi  =  ( (yi>0 ) & (yi<size (yr, 2) -1 ) ) . *yi ; 
zi  =  ceil (  (z-bounds (3,  1) ) /bounds (3, 2)  )  ; 
zi  =  (  ( zi>0 ) & ( zi<size ( zr , 2 ) -1 )  )  . * zi ; 

%  s i z  e  ( imageMap ) 

%  [size (xr,  2),  size ( yr , 2 ) ; max (max (max (xi ) ) ) , 

min (min (min (xi) ) ) ;  max (max (max ( yi ) ) )  , 

min (min (min (yi) ) ) ;max (max (max ( zi ) ) ) ,  min (min (min ( zi ) ) ) ] 
for  r I  =  1 : imgH 

for  angle  =  1 : imgW 

%  temp  =  [xi(rl,  thetal,  1),  yi(rl,  thetal,  l),zi(rl, 

thetal,  1 ) ] 

valid  =  (xi (  rl,  1,  angle)>0  &  yi (  rl,  1,  angle)>0  &  zi ( 
rl,  1 ,  angle) >0) ; 

if  valid 

val  =  imageMap (xi (  rl,  1,  angle),  yi (  rl,  1, 
angle),  zi(  rl,  1,  angle)); 

imageMap(xi(  rl,  1,  angle),  yi (  rl,  1,  angle),  zi ( 
rl,  1,  angle))  =  max(img(rl,  angle),  val); 
end 


end 

end 

end 

%Set  portions  of  the  map  without  data  to  a  value  of  . 5  (unknown 
state) . 

%  imageMap  =  (imageMap  <0).*.5  +  imageMap. * (imageMap>=0) ; 

%  imageMap  =  reshape (imageMap,  bounds (4 , 1 ) *bounds (4 , 3) , 
bounds (4,2)); 
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4.  Display  the  Grid 


function  drawGrid (gridMap,  b,  axis_handle) 

%Generic  Cube  of  Cell  Dimensions 

xcube  =  b  (1, 2) *  [00110  NaN  0  1  NaN  1  0;... 

0  0  110  NaN  0  1  NaN  1  0 ]  ; 
xcube  =  xcube+b  (1 , 1 )  ; 

ycube  =  b(2,2)*  [01100  NaN  1  1  NaN  1  1;... 

01100  NaN  0  0  NaN  00]; 
ycube  =  ycube+b  (2 , 1 )  ; 

zcube  =  b(3,2)*  [11111  NaN  1  1  NaN  0  0;... 

00000  NaN  1  1  NaN  00]; 
zcube  =  zcube  +b(3,l); 

%Traverse  entire  Grid,  Draw  cubes  based  on  the  Probability  contained  in 
%the  cell 

for  i  =  1 : size (gridMap,  1); 

for  j  =  1 : size (gridMap,  2); 

for  k  =  1 : size (gridMap,  3) ; 

switch  logical (true) 

case  gridMap ( i, j , k)  >0.95 

surf (xcube+i*b  ( 1 , 2 ) ,  ycubel j *b ( 2 , 2 ) ,  zcube+k*b (3, 2) , 
'facecolor',  [0  0  0],  'edgecolor',  'none'); 

case  gridMap ( i, j , k)  >=  0.8 

sur f (xcube+i*b ( 1 , 2 ) ,  ycubel j *b ( 2 , 2 ) ,  zcube+k*b (3, 2) , 
'facecolor',  [0.25  0  0],  'edgecolor',  'none'  ); 

case  gridMap ( i, j , k)  >  0.7 

sur f (xcube+i*b ( 1 , 2 ) ,  ycubel j *b ( 2 , 2 ) ,  zcube+k*b (3, 2) , 
'facecolor',  [0.5  0  0],  'edgecolor',  'none'); 

case  gridMap ( i, j , k)  >  0.5 

surf (  xcube  +  i*b ( 1 , 2 ) ,  ycube+j *b (2 , 2 ) ,  zcube+k*b (3, 2 )  , 
'facecolor',  [0.75  0  0],  'edgecolor',  'none'); 
end 

hold  on 

end 

end 

if  mod (i, 10)  ==  0 
i  =  i 

end 

end 

axis  (  [  b ( 1 , 1 )  b  (1, 3)  b(2,l)  b(2,3)  b(3,l)  b  (3, 3) ] ) ; 
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